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ABSTRACT 


Autonomous underwater vehicle navigation is a complex problem of state 
estimation. Accurate navigation is made difficult due to a lack of reference navigation 
aids or use of the Global Positioning System (GPS) that could establish the vehicles 
position. Accurate navigation is critical due to the level of autonomy and range of 
missions and environments into which an underwater vehicle may be deployed. 
Navigational accuracy depends not only on the initialization and drift errors of the low 
cost Inertial Motion Unit (IMU) gyros and the speed over ground sensor, but also on the 


performance of the sensor fusion filter used. 


This thesis will present the method by which an Extended Kalman Filter (EKF) 
was tuned after installation of an IMU in the ARIES Autonomous Underwater Vehicle. 
The goal of installing the IMU, analyzing the navigational results and tuning the EKF 
was to achieve navigational accuracy in the horizontal plane with a position error of less 
than one percent of distance traveled when compared with GPS. The research consisted 
of IMU installation and software modifications within the vehicle to fully realize the 
design goal. Data collection and analysis was conducted through field experiments and 
computer simulation. A significant result of this work was development of a pseudo- 
adaptive algorithm to vary the measurement noise values in selected channels to force a 


desired response in the filter and improve accuracy and precision in the state estimates. 
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I. INTRODUCTION 


A. BACKGROUND 


Autonomous underwater vehicle (AUV) navigation is a complex problem of state 
estimation. Accurate navigation is further made difficult due to a lack of reference 
navigation aids or use of the Global Positioning System (GPS) that could be used to 
establish the vehicles position. Potential mission requirements necessitate accurate 
navigation due to the level of autonomy and the potential range of missions and 
environments into which an underwater vehicle may be deployed. Navigational accuracy 
depends not only on the initialization and drift errors of the low cost Inertial Motion Unit 
(IMU) gyros and the speed over ground sensor, but also on the performance of the sensor 


fusion filter used. 


An IMU helps the problem of state estimation by providing accurate sensory 
inertial inputs that can be used along with a model of the vehicle dynamics. The outputs 
of the IMU take the form of linear accelerations and angular rates that can then be input 
into a system model that will allow for estimation of the vehicles state, in particular the 


vehicle position that is necessary for conduct of various missions. 


This thesis will present the method by which an Extended Kalman Filter (EKF) 
was tuned after installation of an IMU in the ARIES AUV. The goal of installing the 
IMU, analyzing the navigational results and tuning the EKF was to achieve navigational 
accuracy in the horizontal plane with a position error of less than one percent of distance 
traveled when compared with GPS. The research consisted of IMU installation and 
software modifications within the vehicle to fully realize the design goal. Data collection 
and analysis was conducted through field experiments and computer simulation. A 
significant result of this work was development of a pseudo-adaptive algorithm to vary 
the measurement noise values in selected channels to force a desired response in the filter 


and improve accuracy and precision in the state estimates. 


B. MOTIVATION 

Unmanned Underwater Vehicles (UUVs) are being actively pursued in the United 
States Navy as a means to enhance war fighting in the underwater realm. UUVs refer to 
both remotely controlled vehicles and AUVs. These vehicles can be used in a wide range 
of mission functions from Intelligence, Surveillance, and Reconnaissance to Mine 
Warfare to Salvage and Recovery operations as detailed in the Navy’s Master UUV Plan 
(2004). They are a force multiplier and allow the Navy the accomplish missions that may 
be too dangerous or impractical for current practices. “The long-term UUV vision is to 
have the capability to: (1) deploy or retrieve devices, (2) gather, transmit, or act on all 
types of information, and (3) engage bottom, volume, surface, air or land targets (UUV 
Master Plan, 2004).” Figure 1 shows UUVs in use during recent military operations. 


These missions require accurate navigation to perform their tasks. 





Figure 1. Tactical Application of AUV (From: UUV Master Plan, 2004) 


The motivation for this thesis research was to obtain navigational estimates of 
position in the horizontal plane that were within one percent error of the distance 
traveled. This design goal would need to be addressed through both software and 
hardware configuration changes to the navigation architecture of the ARIES vehicle. The 


most significant physical change was in the hardware configuration which would utilize a 


2 


relatively low cost IMU currently used in production of military missile technology. 
Once the gains in positional accuracy from the hardware were realized the navigation 


filter would be tuned in order to achieve the design goal. 


C. THE ARIES VEHICLE 


“The ARIES is used for development of computer architecture, software, sensors 
and navigational hardware for small to medium sized autonomous systems (Healey, 
2006).” It is approximately three meters in length and is fitted with various sensors and 
electronics in order to carry out the development and research noted above. Figure 2 
shows the ARIES being loaded onto the research support vessel Cyprus Sea in Monterey 
Harbor. 





Figure 2. ARIES Operations in Monterey Bay (From: Healey, 2006) 


The vehicles nominal operating speed is 1.2 to 1.5 meters per second developed 
from twin thrusters mounted in the rear. The vehicle operates off of a bank of batteries 


that provide a nominal bus voltage of 60 volts for the vehicle propulsion and hotel loads. 


Vertical maneuvering control for the vehicle is provided by two forward and two rear 
plane surfaces. Steering control is provided by a top mounted twin rudder configuration, 


one forward and one aft. 


Communications with the vessel occur through several antennas on top of the 
vehicle which include 802.11 type wireless digital communication as well as standard 
radio free wave communication. A GPS receiver is mounted on top of the aft rudder to 
allow for receipt of GPS signals when on the surface. Navigation in the ARIES is 
performed without the use of any radio beacons and relies solely on GPS, inertial 


navigation system, and the Doppler speed sensor. 


There are three main compartments in the vehicle that house the equipment 
necessary to run the vehicle. The forward compartment consists of a PC-104 computer 
that operates the sonar imaging obtained from ARIES forward looking blazed array 
sonar. The forward compartment also houses the servos for the forward control surfaces. 
The mid compartment is the largest and houses the vehicle relays for ancillary sensors, 
the battery banks, and the two main computers used for operating the vehicle. There is 
one computer dedicated to executive level process management and mission execution 
and one computer dedicated to tactical execution of commands used for vehicle motion. 
Finally, the aft compartment houses the motors for the thrusters and servos for the aft 
control surfaces, the wireless router for communications and on the forward bulkhead of 


the aft compartment, the IMU. 


D. THESIS STRUCTURE 


The research conducted was an attempt to achieve navigational errors within one 
percent of the distance traveled. This was an iterative approach conducted over an 
approximately one year period that involved data analysis from field experiments coupled 
with computer simulation. The analyses of the errors and modifications made to the 


vehicle from each significant evolution are presented. 


Chapter II will present the theory of the inertial navigation model used, covering 


specifically the operation of inertial motion units and the theory of the EKF as used in the 


4 


ARIES vehicle. Chapter HI will provide the details of the IMU installation into the 
vehicle. Chapter IV will detail the software changes that were implemented over the 
course of this work. Chapter V will present the field experiments and the geometries 
utilized, and the results obtained, presenting in detail the navigational errors. Finally, 
Chapter VI provides thesis conclusions and recommendations for future work. The 


supporting code utilized in this work will be retained in the appendices to this thesis. 
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Il. THEORY OF INERTIAL NAVIGATION 


A. INTRODUCTION 


Inertial navigation systems (INS) have become increasingly widespread in their 
use over the last few decades. This is due in large part to technological gains made in 
computing power and in increasing sensitivity of inertial sensor units made from quality 
manufacturing. The backbone of an inertial navigation system is the IMU, which has 
gained increasing use due to manufacturing techniques allowing the production of small 


and accurate light weight systems as well as larger extremely precise units. 


Coupled with the several order of magnitude increase in computing power that 
has been observed in the last few decades is the development of sophisticated algorithms 
that can process and manipulate the large data streams from an inertial sensor. These 
algorithms allow for increased accuracy in state estimation by using more information 
and increasingly smaller time steps in computing dynamic information. The latter is a 
result of extremely fast processing speeds now capable in computer systems. These 
algorithms allow for inertial navigation of vehicles relying on inputs from inertial sensors 
for navigation such as underwater vehicles and submarines without continuous external 


reference navigation inputs. 


B. INERTIAL MOTION UNIT 


Inertial motion units (also called inertial measurement units) have become 
increasingly popular as measurement devices of vehicle motion for use in inertial 
navigation systems. These units come in a wide variety of forms from simple strap-down 
systems to extremely accurate complex gimbaled or stabilized platform systems. These 
instruments work by sensing the vehicle’s inertial linear accelerations and angular 
rotation rates which can then be sent to a computer for processing with a filter that fuses 
data from different sensors. The intricate details of inertial motion units are well 
published and the following provides a basic understanding of the strap-down style 


system that was used for this research. 


A strap-down style IMU is ideal for a wide range of applications; particularly in 
vehicles where space is limited due to its relatively small size and low weight. These 
units are robust and their use of solid state electronics make them more reliable than 
mechanical gimbaled systems (Yakimenko, 2006). For the details of mathematics and 
mechanization of a strap-down IMU the reader is referred to Siouris (1993). The strap- 
down system uses accelerometers to measure linear accelerations and a set of gyroscopes 
to measure the angular rates of the body. From these accelerations velocity and position 
information may be obtained through integration. The gyroscopes measure the rate at 
which the vehicles attitude changes. From this data the vehicles yaw, pitch, and roll may 


be obtained. A simple illustration of a strap-down IMU is shown in Figure 3. 


Sensor | Bes x 
electronics : - 






object or vehicle 


Figure 3. Basic Strap-down IMU (From: Roth, 1999) 


Cc. EXTENDED KALMAN FILTER 


The EKF was developed to address the problem of utilizing a standard Kalman 
filter, a linear estimator, for problems involving non-linear system dynamics and 
measurements. Motion of vehicles with non-linear dynamics can be made locally linear 


through the use of the Jacobian of the dynamics matrix from the vehicle equations of 


motion. By taking a first-order series expansion of the non-linear equations, the system is 
linearized about a point in time (Bar-Shalom, 2001). The algorithm can then employ the 
estimation techniques of a Kalman filter to produce an estimate of the vehicle state at an 


instant in time. 


1. Assumptions of the EKF 


The EKF utilizes some well known assumptions that are an extension from the 
original Kalman Filter. These assumptions are relevant when comparing filters of 
various types, e.g. the Unscented Kalman Filter, in order to understand the strengths and 
weaknesses of different estimation methods. The EKF assumes that the process noise 
(q(t)) and the measurement noise (v(t)) are white, additive and zero mean. Additionally, 
the process and measurement noises are uncorrelated, i.e. independent of each other. 
Finally, the initial state and its covariance are independent of either the process or 
measurement noise. In summary the process and measurement noise are given as (Bar- 


Shalom, 2001): 


E(q(t)) =0 (1) 
E(qq')=Q 
E(v(t)) =0 a 
E(vv')=R 


2. System Model 


The literature on EKFs is well developed and extensive; therefore, the following 
discussion will focus on the specifics of the EKF as used in the ARIES AUV. For more 
information on EKFs the reader is referred to Bar-Shalom (2001). 


Navigational state estimates in the ARIES vehicle are made by utilizing an EKF 
to predict and correct these states every 0.125 seconds. The state of the vehicle for the 


horizontal plane is contained in an eight state vector as follows (Healey, 1995): 
x(t) =[X,Y,y,r,u,,v,,b,,b,]' (3) 
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Where: 
X = North-South position in Local Navigation Plane 
Y = East-West position in Local Navigation Plane 
Y = Heading 
r = Yaw (Heading) rate 
Ug = Forward speed over ground 
Vy = Lateral speed over ground 
b, = Yaw rate bias 


b, = Heading bias 


The system dynamics and measurement model are given by (Healey, 1995): 


x(t) = f(@x(D) + q() 


(4) 
y(t) =h@(O)+ vO 


For ARIES the vector valued function h(x(t)) is constant and can be represented 


as: 


y() = Cx() (5) 


The EKF may be formulated in either continuous or discrete time; the discrete 
time is necessary for computer application (Healey, 1995). The vehicle states are linked 
through the following vehicle equations of motion and are contained in the dynamics 


matrix. 
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X= u, cos(y)—v, sin(y) 


Y =u, sin(y)+v, cos(y) 


Yy=r 
r=0 (6) 
i, =0 

pv, =0 

b, =0 

b, =0 


Linearizing these equations yields the transition matrix, ®, used to propagate the 


state and covariance matrix between time steps in the discrete model. 


The measurements are associated with the state vector as follows: 


Vp FUg, 

Y2 = Ve; 

y3=yWtb,; 

yg =r+b,; 

ys =X; 

Vets 

or 

y = h(x(t)) = Cx(0) (7) 


The discrete-time filter can then be represented as: 


Xie Wx; ; 


ae 7 OP,,0' + Q 


Lista = PrargC (CPiuC' + R)' 


ienien — Xia T Li Vier - CK) 


itl 


(8) 


x 


Passi =(I- L, OP i 
Where: 
x = Estimate of state 
P = Covariance of states 
Q = Process noise matrix 
R = Measurement noise matrix 
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L = Kalman Gain 


The resulting algorithm provides discrete state updates at each time step and 
predicts the successive time step and then corrects the estimate based on the 
measurements received. When properly tuned and with good quality sensors the EKF 
provides very good estimates of the vehicle state. Tuning the filter is done through the 
choice of the values for the process and measurement noise matrices, based on known 


information about the sensors and/or experience through application of the filter. 


D. NAVIGATION ERROR DEFINED 


The navigational accuracy must be defined to provide a standard quantitative 
measure by which to evaluate the changes made to the ARIES inertial navigation system. 
For this work, only motion in the horizontal plane was analyzed. The absolute error (E ) 
in meters is defined as norm of the error vector taken as the difference between the initial 


GPS position at a time step (7) and the navigation filter estimate at the (i-/) time step. 





est 
II 


2 2 
( x GPS(i) x NavFilter(i-1) ) a (Y, GPS(i) Y, NavFilter(i-1) ) (9) 


The method in which the GPS information is used when obtained in the 
navigation process required the use of the (i-/) position. This is because at time step (i), 
if GPS position information is available this information is inserted into the EKF for the 
X and Y measurements and the estimates are updated, providing a new position estimate 
based on this information. It was necessary to see where the filter thought it was located 
prior to obtaining updated position information compared to the actual vehicle position 
determined by GPS. The assumption was made that GPS information was absolute truth 
for position in order to compare filter performance. The processing of GPS information 
had to be modified slightly in order to obtain increased accuracy by rejecting fixes for 
which the number of satellites visible to the receiver were below a set threshold. This 


modification is discussed further in Chapter IV. The error induced from using the (i-/) 
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position is at most vehicle speed times the time interval of 0.125 seconds, or 0.15 meters, 


which can be considered negligible in this analysis. 


E. THEORETICAL ERROR ANALYSIS 


The ideal position error can be estimated from the manufacturers published gyro 
drift rate for the IMU and some basic assumptions for vehicle dynamics. The Honeywell 


HG1700 has a published drift rate of one degree per hour. Given the dynamics as: 


¥ -u¥ (10) 
P= kt+¥, (11) 
Where: 


y = Cross Track Error Rate 

Y = Heading Error 

 ,=Heading Initialization Error = 1 deg 
k =Gyro Drift Rate = 1 deg/ hr 


U = Forward Vehicle Speed = 1.2m/s 


~ T ~ T we 
Y=| Ydr=| U(k+¥,) dt 
i ae 
-u(eE ser] (12) 
The theoretical cross track error for a thirty minute run is as follows: 


Ve(oe hi 
sec 


eg 
hr 57.3deg 2 57.3de 











2 2 


Y = Distance Traveled = ee ee -1800sec=2160 m 
sec 


The resulting error as a percentage of distance traveled is 2.18%. Based on this a 


design goal of one percent of the distance traveled was set for this research. Figure 4 
13 


illustrates the theoretical cross track error that develops over time based on the above 
formulations. Figure 5 illustrates the percentage error growth over time. Both figures are 
parameterized by the initialization error from using the original compass to align the 
IMU. The effects of the initialization error can be seen on the resulting cross track error 


and error per unit distance traveled. 
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Figure 4. Theoretical Cross Track Error 
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Percentage Error as Function of Distance Traveled 
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Figure 5. Theoretical Error Growth 


As can be seen from these two figures the amount of error in position estimates 
that will develop is strongly dependent on the initialization error of the IMU and to some 
degree the drift rate of the gyro. Therefore, it is critical that the navigation filter learn the 
heading bias quickly in order to compensate for this initialization error. The rate of 


learning must be balanced against the overall performance of the filter. 
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ll. HARDWARE IMPLEMENTATION 


A. INTRODUCTION 


The ARIES vehicle originally used a compass to provide heading and a Systron 
Donner Motion Pak IMU provided gyro rate inputs to the navigation filter. The compass 
was a Honeywell HMR3000 magneto-restrictive compass and provided reasonable 
outputs for the sensor (Marco et al., 2001). However, a compass has the disadvantage of 
induced errors from the true heading due to magnetic field variations caused locally 
around and within the vehicle. Additionally, there will be a deviation from true North 
based on location of operations that must be corrected for. As a result the compass would 


lose accuracy over time and produce significant navigational errors. 


The accuracy achievable with the compass is a direct result of the ability to 
conduct calibrations with the compass and vehicle as well as a reasonably accurate and 
updated deviation table used for corrections. Another difficulty in utilizing the compass 
was that the heading bias learned was dependent on heading and thus required extensive 
use of the deviation tables (Kragelund, 2006). The calibrations, if successful, 
significantly reduced the error that resulted in the navigation filter but they were difficult 


to obtain good runs and required a considerable amount of time. 


B. INERTIAL MOTION UNIT INSTALLATION 


For this work a Honeywell HG1700 IMU was purchased and installed in the 
vehicle. The HG1700 is a strap-down type of IMU and is considered a low cost military 
grade production unit. The location was selected to place the unit as close to body center 
as possible without displacing elements of the vehicle previously installed. The inertial 
motion unit and associated electronic circuitry represent the only hardware modification 


associated with this work. 


The IMU was installed in the aft compartment mounted vertically against the 


forward bulkhead. Figure 6 illustrates the installation of this unit. 
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Figure 6. IMU Installed in ARIES 


The unit was centered on the body lateral and vertical axis and mounted in place 
for the output values to correspond directly to the body local x, y and z axis. The body 
defined axis’ for the ARIES uses traditional orientations with the x-axis along the body 
centerline, the y-axis is athwartships on the body and the z axis is defined positive in the 


downward direction. 


The unit utilizes +/- 15 volt DC and +5 volt DC input power for the associated 
electronics. This power is tapped off of the main battery bus power and stepped down 
from 60 volts to the required input power using two Calex DC-DC converters shown in 
Figure 6. The larger one supplies the +/- 15 volt DC power while the smaller supplies the 
+5 volt DC power (Kragelund, 2006). 


18 


IV. SOFTWARE IMPLEMENTATION 


A. INTRODUCTION 


Once the hardware was installed and the results of the hardware were analyzed, 
attention was turned towards the software architecture of the navigation filter realizing 
that to achieve the design goal of one percent of distance traveled both hardware and 
software improvements were going to be required. The approach taken was dual in 
nature utilizing simulation and field experiments. The EKF presented in Chapter II is 
embedded in the navigation code and is often referred to as the navigation filter. The 


terms EKF and navigation filter are synonymous with respect to this work. 


This chapter will present the changes made to the system software as well as what 
was in use for the operating system. Additionally, the method in which MATLAB® was 
used to enhance the result of this research for the UKF and EKF will be shown. The 
order of the following sections represents modifications as performed in chronological 
order. The results obtained from these changes are shown in Chapter V in the same 


chronological order 


1. Vehicle Operating System 

The ARIES vehicle utilizes a QNX real time operating system with code written 
in ANSI — C for both the tactical and executive computers. The system operates on an 8 
hertz (Hz) data cycle and the measurements obtained for state information are 
asynchronous. GPS data, when available, have a frequency of 1 Hz. Speed information 
from the RDI Doppler is provided at a frequency of 2 Hz. Lastly, the IMU provides data 
at 100 Hz but is selectively chosen over each 8 Hz cycle. This will be discussed more in 


section B. 


2, MATLAB® Simulation 
For simulations, a MATLAB® code was developed based on previous work by A. 


Healey, which modeled the navigation filter in the vehicle and utilized data obtained from 
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experiments. This code was then manipulated to determine effects on navigation 
performance from changes to the filter. The MATLAB® version of this code is 


contained in Appendix B. 


3. Unscented Kalman Filter 

In recent years there have been many attempts in the literature to improve upon 
the EKF due to the difficulties in implementation and tuning. The EKF requires 
reasonable determinations of the process and measurement noise for the system in order 
to implement; this can be rather difficult and may require much time to achieve optimal 


performance. 


The Unscented Kalman Filter (UKF) was developed by Simon Julier and Jeffrey 
Uhlmann (1997) as an alternative method to the EKF. This filter characterizes the mean 
and covariance parameters by the use of a set of discrete points (Julier et. al, 1997). 
These points are then propagated through time and the mean and covariance are 
reconstructed to provide the updated estimate of the state. This filter does not require that 
the process or measurement noise be Gaussian and the Jacobian is not required to be 
calculated for the system. These last two are the major advantages for this type of filter 
over the EKF. The literature indicates that this particular application of Kalman Filter 
has shown better results for particular applications when compared to the EKF. Based on 
this latter claim a UKF was attempted for implementation in the navigation filter of the 


ARIES AUV. 


A UKF, based on Julier and Uhlmann’s work was developed, in MATLAB® for 
ARIES navigation by Dr. Hag Seong Kim, a research assistant with the AUV Research 
Group assisting with this work, utilizing data from experiments. The MATLAB® 
version of this filter is contained in Appendix C. This filter was run for a multitude of 
values for process and measurement noise and compared to the in-vehicle filter. Figure 7 
shows the results of the UKF developed when compared to the EKF contained in the 


vehicle. 
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Figure 7. UKF vs. EKF for ARIES Run — 9/7/05 


It was determined that, for this vehicle and application, the UKF produced results 
and errors on the same scale as the EKF without sufficient improvement in accuracy. It 
was noted that the UKF provided tighter estimates of position when updated with GPS 
and the same results can be observed with the speed components where less filtering 
occurs and more of the dynamics are retained in the estimates. However, when later 
work was performed with tuning the vehicle EKF, better results were obtained. 
Therefore, further work in code development and implementation in the vehicle was not 
warranted for the UKF. It can be said that the implementation of the filter was slightly 
easier since derivatives were not necessary, however, the same difficulties, 1.e., 
determining appropriate process and measurement noise matrices, encountered in tuning 


the EKF, would be the same for the UKF. 


4. Vehicle Code 


There were two primary areas of vehicle code that were both developed and 


modified in order to install and make usable the data from the IMU. The first was an 
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operating code for the IMU that would control and convert the data flow from the IMU to 
be used by the navigation filter in the onboard computers. The original IMU code was 
developed by Jack Nicholson, CAPT, USN during the installation of the IMU. This code 
is contained in Appendix D. The second was the navigation filter code which contained 
the routines of the EKF that developed vehicle state information at each time step. A 


copy of this code is contained in Appendix E. 


The ARIES operating system operates asynchronously accepting sensor inputs 
from multiple locations at different frequencies. The vehicle operates at a high enough 
time constant to be able to ensure no aliasing occurs among the sensors and the data. The 


vehicle time constant can be estimated to be: 


pol pees (13) 
U-1.5m/s 











This leads to a dynamic frequency for the vehicle of: 


1 
ine = ens 


= =0.5 Hz (14) 
2 sec 


Therefore according to the Nyquist Sampling Criterion: 
Pp as =1Hz (15) 


The conclusion here is that as long as the sensor inputs occur at a rate greater than 
1 Hz, the vehicle dynamics are slow enough to ensure that adequate sampling of those 
dynamics by the sensors are sufficient. This is important from the standpoint of utilizing 


IMU data to be discussed shortly. 


B. INERTIAL MOTION UNIT CODE 


The IMU code used by the ARIES was developed to handle the flow of data 
obtained from the Honeywell HG1700. The IMU provides inertial information for linear 
acceleration and angular rotation rates in three dimensions for the vehicle in the local 
body fixed frame. The code converted the readings into the local navigational tangent 


plane and corrected the measurements for the Earth’s rotational rate. While three 
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dimensional inertial information can be obtained from the unit, it was the heading rate 
that was of primary concern for the navigation filter. The heading rate was integrated 
within the code using a simple Euler integration scheme over the 8 Hz time interval. The 
simple integration scheme was justified based on the short time interval compared with 
the rate of vehicle motion. The result of this integration was a sufficiently accurate 


heading input to replace the compass heading previously utilized. 


The IMU code provides for the necessary rotations and adjustments that are 
necessary to obtain truly inertial measurements from the IMU. The accelerometers and 
gyros associated with the IMU measure the acceleration and rotations felt upon the 
sensor, and not all of the measurement is due to body motion of the vehicle. The IMU 
will sense the rotation of the Earth frame in which the vehicle is moving. The 
measurements thus obtained from the IMU must be corrected for the Earth’s sidereal 
rotation rate. Once the IMU information has been rotated and corrected it can then be 
fused with other data obtained in the same reference plane with which position estimates 


can then be made. 


One of the primary calculations needed is to rotate the Earths angular rotation rate 
of approximately 15.04 degrees per hour into the body fixed frame. This is necessary to 
obtain purely inertial measurements for the vehicle since the local navigation plane is 
rotating. Figure 8 provides the illustration showing the relationship between the 
navigation plane the vehicle moves in with the Earth’s rotating reference frame. The 
local navigation plane oriented in the North-East-Down configuration rotates at the 


Earths sidereal rotate and this rotation is felt upon the gyros in the IMU. 
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Figure 8. Earth Frame of Reference (From: Yakimenko, 2006) 


Once rotated, the elements corresponding to the Earth’s rotation can be removed 


from the measurements for angular rates within the IMU. Only those elements needed to 


obtain the heading rate were calculated. The equations and process used are outlined as 


follows and are contained in the IMU code in Appendix D. 


1) The Earth’s sidereal rotation vector [0,0,Q]’ is rotated into the body fixed frame 


[Peqete]’ using the current geodetic latitude of the vehicle and the vehicle body 


Euler angles (@,0,y). 
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q. =|-cos(g)-sin( y) -cos(Lat)-sin(¢) - cos(4) - sin(Lat)]-Q (16) 


[= [(sin(¢) -sin(y)+cos(¢)-sin(@)-cos(y))-cos(Lat)-cos(¢@) -cos(@) - sin(Lat) | -Q (17) 


2) 


3) 


4) 


The elements of the Earths sidereal rotation rate are then removed from the 


angular rate measurements from the IMU gyros [Pout, Gout, Tout] ’- 


Vout aaa Vout e} q. (18) 
Tout ou ~ Te 


The adjusted angular rate values which represent the true angular rate of the 


vehicle with respect to a true inertial frame can be used to obtained the heading 


rate (y ). 


—_ sin(¢) se cos(@) ee 
cos(@) cos(@) 








(19) 


The heading rate is then integrated using Euler integration with the result being 


an updated heading value at the i+1 time step. 


Ay =w-At (20) 


Win =W,tAy (21) 


The output of the IMU is at 100 Hz, i.e. there are 100 readings of all channels per 


second. Originally, twelve readings were averaged over an 8 Hz period in order to align 
the IMU output with the operating system. Checks for data stream integrity were utilized 
in the form of identification of a unique hexadecimal identifier that indicated a new 44 bit 
data stream. During the research period it was identified that the data streams were 
becoming corrupted and providing invalid information over some of the read cycles. 


This was contributing to errors propagating into the position estimates. 


25 


It was determined that, due to the errors developing in the data streams over an 8 
Hz period, instead, a single 44 bit data stream would be used as the average for the 
interval. Additionally, more rigorous data checks would be placed on the data to ensure 
that it was a complete and valid data message. These checks included continuing to 
identify a new message by the unique identifier and the use of a checksum at the end of 
the data stream to ensure that the preceding 42 bits of data were correct and belonging to 
the same message. The new message identifier and checksum are IMU manufactured 
features in the data message. The use of a single reading of the 100 Hz data in each 8 Hz 
cycle was reasoned to be appropriate because the vehicle dynamics did not change 
rapidly enough to have significantly different readings from interval to interval and the 
sampling frequency was still greater than the system dynamics frequency as discussed 


above to justify this averaging scheme. 


C. NAVIGATION FILTER CODE 


The majority of the code changes were made in the navigation filter that contains 
the EKF in the vehicle. The next few sections will provide the methods by which the 


navigation filter code was changed with the results shown in Chapter V. 


1. Gyro Rate Bias 


The EKF originally used was developed for eight states as previously discussed. 
After examination of the first run utilizing the IMU on September 7, it was observed that 
the filter was learning a gyro rate bias that varied greatly and was significantly greater 
than the manufacturer specified maximum gyro drift rate of one degree per hour. Thus, it 
appeared as if the filter was developing a larger gyro rate error than the maximum 
specified. The correction for this was zeroing out the associated gyro rate bias state in 
the filter. This was accomplished by setting the rows and columns of the observation 


matrix to zero and to set the gyro rate bias state to zero during the calculation cycle. 
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2: Adaptive Process and Measurement Noise Algorithms 


There has been much work performed to find optimal algorithms that can 
adaptively determine the values for the process and measurement noise matrices. Much 
of the available literature researched indicates that most of the work is based on ideas 
proposed by Mehra (1970) or work by Myers and Tapley (1976). The method originally 
proposed by Mehra (1970) as modified and presented by Busse et al. (2002), was 
attempted for use in the navigation filter for the ARIES. 


Successful results have not yet been obtained for this application. The literature 
suggests that the algorithms are highly dependent on tuning factors in the schemes. 
Therefore, a purely adaptive routine has not been found for this particular application and 
the pseudo-adaptive routine developed in this work continues to provide the best results 


with respect to navigational accuracy. 


3: Pseudo-Adaptive Measurement Noise Matrix 


The last major innovation to the EKF was the tuning of the weights for 
measurement noise values used in the algorithm. Prior to this work the values for both 
process noise and measurement noise values were assumed to be constant and obtained 
experimentally over significant periods of operation with the vehicle and as such were a 


very good first guess to the optimal values to use in the filter. 


The measurement vector is a six state vector in the navigation model consisting of 
position, both X and Y, forward and lateral speed over ground and heading and gyro rate. 
It was postulated that by selectively forcing the weights of specific states up or down that 
the overall filter would place more or less emphasis on the measurements for that state. It 
has been observed that the Kalman Gains are directly proportional to the weights used for 
the process noise and inversely proportional to the weights for the measurement noise 
values. 


Kalman Gain (L) « e (22) 


2] 


Equation (22) was used as a guide to adjusting the weights from their previous 
values using the simulation EKF in MATLAB® and the effect on position estimates 
compared to GPS positions were evaluated for all available data sets. Specifically, the 
pseudo-adaptive routine developed reduced the measurement noise weighting values used 
for the position states, thereby forcing the filter to rely more heavily on the information 
from the GPS fixes. The measurement noise matrix is 6x6 and only the diagonal values 
associated with the fifth and sixth state of the measurement vector corresponding to the 
position information is altered each time step through the filter based on the number of 


satellites seen by the GPS receiver. 


The algorithm developed for the code altered the measurement noise values for 
the position states by reducing them by an order of magnitude as the number of satellites 
visible from the GPS receiver increased. The original values were used for three or less 
satellites visible, then reduced an order of magnitude for four satellites, the first position 
information in which a GPS fix is declared and reduced another order of magnitude for 


five or more satellites. 


This combination was run on several of the data sets from the May, June, and July 
timeframe of 2006 and the effects on estimated position when compared with the current 
in-vehicle output as well as the GPS positions. Figure 9 shows the results of this 
algorithm on the data obtained from Run 3 on July 25. The improved filter performance 
can clearly be seen when compared with the GPS position after a two kilometer run. 
After simulation runs on different data sets, confirming the results observed in Figure 9, 


the algorithm shown in Figure 10 was implemented in the vehicle for field evaluation. 
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Figure 9. Simulation with Pseudo-Adaptive Algorithm of Run 3 - 7/25/06 





// SPK 081606: Set elements of R Matrix based on 
NSv (as verified by SRV by 08/2006) 





if (NSv <= 3) 
nu[5]=1.0; 
nu[6]=1.0; 

else if (NSv == 4) 
nu[5]=0.1; 
nu[6]=0.1; 

else if (NSv >= 5) 
nu[5]=0.01; 
nu[6]=0.01; 








/* Update Diagonal R Matrix */ 
for (1=1; 1<=7; ++i) 
R[i][i] = nul[il; 














Figure 10. Pseudo-Adaptive Algorithm for Measurement Noise 


Originally, the filter and the GPS software utilized a set point of three GPS 
satellites as being sufficient fix quality to use in the filter, however, it was observed that 
the use of three satellites produced a significant error in the solution as shown in Figure 


11. Therefore, the GPS code was changed to declare GPS information available if the 
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number of visible satellites to the GPS receiver was greater than or equal to four 
satellites. This higher threshold ensured the GPS receiver was obtaining higher quality 


fix information. 
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Figure 11. Three vs. Four GPS Satellite Fix 


In addition to changing the measurement noise values for the position states, 
computer simulations were run altering only the speed noise weighting values to observe 
the effects on filter performance with respect to position estimates. The weighting values 
were changed individually for the forward speed measurement and the lateral speed 
measurement by orders of magnitude until satisfactory performance was achieved 
qualitatively in simulation. These values were then checked against other data sets to 
ensure that acceptable performance was maintained in terms of accurate position 
estimation compared with GPS. The result was that the forward speed measurement 
noise weighting value was reduced by two orders of magnitude and the lateral speed 
measurement noise weighting value was altered by a single order of magnitude, placing 


more reliance on the speed measurements overall. 
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Observing the resulting filter speed estimates when compared to actual 
measurements it was seen that the effect on the speed states was reduced smoothing of 
the data. The speed measurements from the RDI Doppler and the estimations of vehicle 
speed from the EKF in-vehicle compared to the new estimates using the modified 
weights are shown in Figure 12 and Figure 13. The red cross marks are the measured 
values for speed from the RDI Doppler speed sensor, the blue dots are the in-vehicle EKF 
estimates from that run and the green dots are the new estimates from the simulation EKF 


after modifying the noise values. 
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Figure 12. Forward Speed Filter Results of Run 3 -7/25/06 
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Figure 13. Lateral Speed Filter Results of Run 3 -7/25/06 


The observation drawn here is that what appears to be noise in the measurements 
and captured as such in the system model could be actual slight changes in vehicle speed 
due to external forces, possibly from wave or current action or alterations in which the 
thrusters propel the vehicle through the water. It would thus introduce slight errors to 


model this information as noise vice actual variations in the speed measurement. 


An additional feature of the fusion filter with the pseudo-adaptive algorithm is 
that it is more responsive to the heading bias learning process. This phenomenon can be 
observed in Figure 14; while not as smooth, it converges to the actual necessary 
initialization error quicker than previous runs due to the increased weighting on the 
position measurements. The oscillatory nature of the heading bias state early in the run is 
from speed information that is approximately zero when the vehicle is on the surface at 
the beginning of a run awaiting mission execution commands but with the navigation 


process running. 
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Figure 14. EKF Bias Learning with Pseudo-adaptive R Matrix — 7/25/06 


Lastly, it was observed that once the vehicle started its thrusters and began a 
mission with an increase in speed the heading bias converges quickly to a steady state 
value that is very close to the final learned heading bias. This is due to the fact that 
accurate position information is still being obtained via GPS fixes prior to submerging 
and there is a non-zero speed component with a relatively accurate heading input. The 
heading bias as the only unobserved state during this time frame which converges rapidly 


within this estimator with all other states observed. 


One last navigational modification was made based on this last observation after 
the August 24 run. A time delay of 10 seconds was inserted into the executive process of 
ARIES to delay submerging after thrusters were initiated. With this delay, the filter 
would have all the state information necessary for accurate heading bias estimation 
thereby eliminating needs for subsequent pop-ups. The value of this would be enhanced 
mission effectiveness by eliminating the need for a GPS fix to correct the heading bias 


prior to entering an operating area to conduct its mission. 


33 


THIS PAGE INTENTIONALLY LEFT BLANK 


34 


V. EXPERIMENTS AND RESULTS 


A. INTRODUCTION 


In order to understand the effects of the implementation of the IMU and 
modifications to the vehicles software architecture presented in Chapters III and IV, 
experiments with the vehicle were conducted in Monterey Bay with the ARIES vehicle 
from September 2005 through September 2006. These tests were established to run the 
vehicle in a controlled geometry with varying GPS pop-ups in order to quantify the 
navigational position error between the filter estimate and the vehicles position as 


determined by the GPS receiver. 


B. EXPERIMENT GEOMETRIES 


Experiment geometries were maintained rather simple and consistent to provide a 
basis for rapid qualitative comparison in-situ as well as post analysis quantitative 
comparison. This was done by utilizing a straight run of lengths of one kilometer initially 
and then eventually expanding to two kilometer runs and finally a four kilometer run to 
validate the findings. The runs were oriented on a North-South axis for simplicity of 
track planning and safety of vehicle in order to run parallel to the shoreline in Monterey 
Bay. The majority of runs were set to run at a depth of three to four meters unless other 
testing requirements dictated that the vehicle run on altitude control utilizing the bottom 


topography vice a commanded depth input. 


1. GPS Pop-up Maneuvers 


The number of GPS pop-ups varied from as many as approximately eight on the 
early runs to two on the final runs towards the end of the test period. The use of many 
GPS pop-ups allowed for statistical quantification of the mean error between filter 
estimate of position and actual position. Additionally, the use of many pop-ups allowed 
for analysis of filter performance with respect to estimating the gyro rate bias and the 


heading bias throughout the run. 
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2. Geometry Modifications 


There were two modifications to the geometry described above during the 
experimental phase. First, a multi-heading, multi-turn geometry was utilized in order to 
qualitatively observe performance of the vehicle IMU and filter under a rather intense 
maneuvering scheme. This run can be seen below in Figure 15. The IMU performed 
excellently as a heading reference on multiple courses over this run without incurring 
additional errors that otherwise may have been incurred with a compass. Secondly, the 
final four kilometer validation run was performed on an East-West cardinal heading 
following a two kilometer North-South run to demonstrate that the results were 
independent of vehicle heading. Additionally, this ensured no artificial bias may have 


existed in the results obtained by the selection of repeated North-South runs. 
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Figure 15. Multi-Heading Run Geometry — 5/18/06 
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C. EXPERIMENT RESULTS 


The following sections will present the results of the modifications discussed in 


Chapters III and IV following the same chronology. 


1. Original Navigation Filter 


The original navigation filter utilized a compass as the heading reference input 
and the navigational errors incurred were relatively large. Figure 16. and Figure 17. 
show the evolution of the mean errors with each major change to the navigation filter as 


well as the mean error per unit distance traveled. 
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Figure 16. Evolution of Navigation Filter Errors 
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Figure 17. Expanded Evolution of Navigation 


Figure 18 and Figure 19 illustrate typical tracks from the use of the compass as a 


heading input to the navigation filter. 
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Figure 18. Compass Based Track — 6/10/05 
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Figure 19. Compass Based Track — 8/9/05 


The navigational errors were large and highly dependent on the quality of the 
compass calibration and the ability of the heading bias learning process to correctly 
obtain the initialization error. With the compass as the heading reference, the baseline 


mean error was 70.59 meters and the error per unit distance traveled was 66.67%. 


2. Initial Hardware Modification 


The installation of the Honeywell IMU replaced the Systron Donner Motion Pak 
IMU as the source of the gyro rate which when integrated would provide the heading 
input for the filter to produce better estimates of position based on the equations of 
motion. The first run with the IMU saw significant improvements in the quality of the 


position estimates. Figure 20 shows the results of this run. 
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Figure 20. Initial Post-IMU Install Run — 9/7/05 


There was a significant difference in navigation performance with the 
implementation of the IMU. Overall mean navigational errors were reduced to 8.04 
meters and the error per unit distance traveled had been reduced to 3.04 % for this initial 


run with installation of the IMU. 


3. Gyro Rate Bias Modification 


The corresponding reduction in navigational error was an order of magnitude. 
Analysis of the September 7th run revealed that the rate bias state was producing rather 
large values, which when compared to the manufacturers published gyro drift rate for the 
IMU of one degree per hour indicated that there were additional errors being induced or 
captured by this state through the vehicle dynamics. The gyro rate biases obtained from 


the initial runs with the IMU are shown in Figure 21 and Figure 22. 
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Figure 21. 
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Figure 22. 
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Based on the observations of Figure 21. and Figure 22. above it was determined to 
force the rate bias state to zero within the filter to eliminate erroneous learning and thus 
introducing errors into the filter. The mean positional errors during the time period that 
the rate bias learning was activated and with the IMU installed was 19.23 m with an 
associated 8.69 % error per unit distance traveled. There was significant variability in the 


results as seen from the raw data in Table 2 of Appendix A. 


With the rate bias state set to zero in the filter as discussed in Chapter IV, the 
ARIES was run again on May 25, June 14 and June 15 without any other changes to the 


navigation filter. Figure 23 is a typical result of the runs during this time period. 
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Figure 23. Typical Run with Rate Bias set to Zero — 6/14/06 


The mean navigational errors during this period were 16.09 meters, which is not a 
significant change but the mean error as a percentage of the distance traveled decreased 
to 5.63 %. Again, there was much variability observed in the results but the trend is 


increasing downward. 
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4. IMU Code Averaging Scheme 


During the June runs it was discovered that there were errors being introduced 
into the 44 bit data streams containing the measurements from the IMU. These errors, 
under the original architecture of the code, were being averaged over the 8 Hz period. 
The erroneous heading rate was then being integrated for heading for use by the 
navigation filter. The source of the errors was not discovered and the IMU code was 
changed as discussed Chapter IV. The ARIES was run on July 19 and July 25 with the 


results shown as follows in Figure 24 and Figure 25. 
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Figure 24. ARIES Run with IMU change — 7/19/06 
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Figure 25. ARIES Run with IMU change — 7/25/06 


A noticeable improvement was realized with this methodology which utilized a 
single good sample from the 8 Hz calculation interval. The underlying assumption made 
was that the true vehicle dynamics did not change quickly enough and that a single 
measurement made over the interval would be representative of the interval. During this 
period of runs, the mean navigational error improved to 7.92 meters and error per unit 
distance traveled was 0.81 %. The overall goal of one percent of distance traveled had 
been achieved but a review of Table 4 shows that there were some values that were right 


above the desired level. 


3; Pseudo-Adaptive Measurement Noise Modification 


The result of implementing the pseudo-adaptive noise algorithm were errors 
consistently less than one percent error over the distance traveled. An initial two 
kilometer run was conducted on August 24 to verify the results compared to previous 


runs of the same geometry and to validate what had been observed in simulation shown 
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in Figure 9. The total error after the two kilometers was 5.36 meters for a resulting 


percentage error of 0.23% of the distance traveled. This run is shown below in Figure 26. 
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Figure 26. 2 km Run with Pseudo-Adaptive Algorithm — 8/24/06 


A second run was then conducted to confirm the results obtained. For this run the 
geometry would be rotated so that the runs would occur on East-West cardinal headings. 
Additionally, the run would be doubled to four kilometers to observe how the filter 
handled the accumulated error due to the drift of the IMU gyros during this extended run. 


The four kilometer run is shown in Figure 27. 
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Figure 27. 4 km Run with Pseudo-Adaptive Algorithm — 8/24/06 


The result for this run was an accumulated position error of 14.16 meters over the 
four kilometer run, resulting in a percentage error of 0.33%. This geometry demonstrated 


that the accuracy of the navigation was independent of any particular track heading. 


6. Surface Time Delay Implementation 


An initial ten second surface delay was inserted into the operating profile for the 
vehicle and tested on October 17. This based on observations made during simulation on 
the July 25 run as discussed in Chapter IV. The results of the surface delay are shown 
below in Figure 28. An enhanced view of the initial surface run is illustrated in Figure 
29. The heading bias with accurate position information and non-zero speed information 
approaches a final steady state value, thus validating the hypothesis discussed in Chapter 
IV. The result of learning the bias quickly before submerging is that, if given the proper 
time, the filter estimates will converge with actual position by GPS which can be 


observed in Figure 30. 
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Enhanced view of Bias Learning — 10/17/06 


Figure 29. 
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Figure 30. EKF Estimate and GPS Convergence — 10/17/06 


The result of implementing the surface time delay and thus learning the heading 
bias while still surfaced is observed with an increase in accuracy of the initial GPS pop- 
up maneuver. The filter estimate when compared to GPS position for the first GPS pop- 


up can be seen in Figure 31. 
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Figure 31. Initial GPS Pop-Up Maneuver with Surface Time Delay — 10/17/06 


48 


The work performed in this research has resulted in excellent navigational 
accuracy for the ARIES AUV. It will allow the ARIES to be used for work in which 
precise position information must be made available to the vehicle and will allow 


research that had been unable to be accomplished. 
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VI. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 


This research demonstrated that good navigational accuracy could be achieved 
through the use of a low cost IMU coupled with a properly tuned EKF. A one percent 
error as a function of distance traveled was achievable in this research. This was shown 
to be possible through the use of a higher quality IMU that had a lower gyro drift rate 
thus inducing smaller errors over time in the heading input for the equations of motion 


for the vehicle. 


A pseudo-adaptive routine was developed that demonstrated how information 
obtained from measurements could be maximized based on the quality of the information 
by properly weighting the measurement noise values for certain states. In this manner 
better estimates of position were obtained as compared to the GPS positions with overall 
smaller errors. The better estimates of position were the result of a faster convergence to 
a particular heading bias for a given run. The pseudo-adaptive routine forced the heading 
bias state in the system to be more reactive and dynamic due to changes in the position, 


heading and speed states. 


It was also shown how a navigation filter could be enhanced by changes in 
operating procedures. This was demonstrated through the surface time delay in which a 
highly reactive heading bias state could converge quickly to the necessary value by 
increasing speed while still on the surface receiving GPS information during the initial 
period of a run. The result is that with the heading bias learned prior to initial 
submergence, subsequent vehicle pop-up maneuvers to learn the heading bias could be 


eliminated. 


B. RECOMMENDATIONS 


This research stimulated many questions regarding the accuracy of underwater 


vehicle navigation. Some of these questions were answered by this thesis, more remain. 


py | 


This work was based on establishing a GPS position as ground truth; however, there are 
inaccuracies within this system that would leave an absolute position error that should be 


dealt with. 


The navigation filter during the course of the research was modified from an eight 
state filter to a seven state filter through zeroing the rate bias state. The code and filter 
should be redesigned to remove the zeroed state for the rate bias into a better defined 
seven state model. 

This work focused on utilizing a single specific channel from the IMU, i.e. the 
yaw rate that was then integrated to provide a good heading reference. The IMU outputs 
three dimensional body linear accelerations and angular rates that could be used to further 
enhance the accuracy of the navigational estimates in both the horizontal and vertical 
planes. The method proposed based on current system architecture is to integrate the 
accelerations in the IMU code at 100 Hz and output the velocity components at 8 Hz 
intervals. These velocities could then be used in place of the velocities provided by the 
RDI Doppler in the EKF. 

Adopting a two IMU system and removing the RDI Doppler as the primary speed 


sensor. Eliminating the RDI Doppler as an onboard sensor has the following benefits: 
" Hotel load reduced, increasing mission endurance. 
= Reduced electrical noise impacts on Forward Look Sonar. 
= Reduced capital cost. 
o $35k (RDI Doppler) + $15k (IMU) vs. 
o $15k (Primary IMU) + $15k (Backup IMU) 
= Eliminates water column constraints by eliminating need to see ocean bottom. 
* Increases payload space for other components or reduces overall size of AUV. 
«Enhanced mission flexibility due to above. 


This work discovered that a pseudo-adaptive measurement noise scheme worked 


rather well in reducing the overall navigational error. While some work was performed 


a2 


to adapt an algorithm by Busse et al., (2002) into the EKF, little success was made and 
the filter solutions diverged. Additional work ought to be performed to determine an 
adaptive process and measurement noise algorithm that will perform within the current 


architecture of this EKF application. 


There is potential work that warrants attention in post-process smoothing of the 
vehicle track. In this manner by using more precise information from position 
measurements made through GPS the track that the vehicle actually ran between GPS 
pop-up maneuvers may be estimated. It can be seen from previous figures that the 
vehicle bases its control action from position estimates made through the navigation filter 
and that there are discontinuities in the solution from the (i-/) time step to the (7) time 
step at which a GPS measurement is made. By using a fixed-interval smoothing routine 
as proposed by Bar-Shalom (2001) the vehicle track may be post-processed for a more 


accurate representation of true vehicle position between GPS fixes. 


a) 
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APPENDIX A: EXPERIMENT DATA 


The mean errors and associated errors per unit distance traveled for runs analyzed 
during this research are summarized in tables 1 through 5. The tables are divided into 


each specific evolution of the vehicle for navigation improvements. 












































Date NavD File d File Delta X Delta Y Error (m) Error % 
Dist 
Traveled 
6/10/2005 | NavD_061005 01 | d061005 01 -45.49 -65.53 79.77 116.29 
50.04 -60.47 78.49 18.33 
Totals 4.55 -126.00 158.26 134.62 
Means 2.27 -63.00 
StDev 67.55 3.57 0.90 69.27 
8/9/2005 | NavD_080905 01 | d080905 01 -1.15 28.35 28.37 54.94 
-2.63 49.49 49.56 81.63 
-12.39 116.11 116.77 62.15 
Totals -16.17 193.96 194.71 198.73 
Means -5.39 64.65 
StDev 6.11 45.80 46.15 13.80 




















Table 1. Pre-IMU ARIES Data 
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Date NavD File d File Delta X Delta Y Error (m) Error % 
Dist 
Traveled 
9/7/2005 | NavD_090705_01 | d090705_05 4.27 9.19 10.14 4.84 
-0.14 5.95 5.95 1.24 
Totals 4.14 15.14 16.09 6.08 
Means 2.07 7.57 
StDev 3.12 2.29 2.96 2.54 
5/12/2006 | NavD_051206 02 | d051206 02 3.85 -0.07 3.85 1.88 
-0.08 0.87 0.87 0.48 
-0.32 -1.44 1.47 0.67 
0.58 -4.73 4.77 2.34 
0.81 -6.24 6.30 3.17 
-1.85 -12.34 12.48 6.25 
NavD_051206 03 | d051206 03 14.22 -55.17 56.97 29.13 
-25.92 49.32 55.71 27.52 
7.39 53.30 53.81 23.99 
7.89 -1.62 8.06 3.48 
1.94 -32.71 32.76 17.09 
-10.45 -25.79 27.82 14.33 
8.32 -31.07 32.16 17.26 
NavD_051206 03 | d051206 04 0.95 48.04 48.05 8.32 
Totals 7.33 -19.65 345.08 155.92 
Means 0.52 -1.40 
StDev 9.56 32.24 21.89 10.31 
5/18/2006 | NavD_051806 03 | d051806 04 3.37 -1.32 3.62 0.71 
-6.00 -2.70 6.58 3.60 
-0.93 -0.47 1.04 0.42 
-6.59 10.30 12.22 7.15 
Totals -10.15 5.81 23.46 11.88 
Means -2.54 1.45 
StDev 4.68 5.97 4.80 3.13 
Table 2. Post-IMU Installation ARIES Data 
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Date NavD File d File Delta X Delta Y Error (m) Error % 
Dist 
Traveled 
5/25/2006 | NavD_052506 03 | d052506 03 1.85 0.32 1.88 0.26 
NavD_052506_04 | d052506 04 4.12 -2.47 4.80 0.61 
Totals 5.97 -2.15 6.68 0.87 
Means 2.98 -1.08 
StDev 1.61 1.97 2.07 0.25 
6/14/2006 | NavD 061406 02 | d061406 02 2.33 -8.76 9.07 1.74 
5.10 -3.18 6.01 1.36 
NavD_061406_04 | d061406_ 04 10.30 25.36 27.37 7.88 
1.86 16.00 16.10 6.43 
2.00 17.89 18.01 8.54 
-0.11 12.32 12.32 5.95 
15.45 14.11 20.93 8.19 
-1.29 30.81 30.84 15.27 
-5.84 19.96 20.80 11.12 
-2.02 11.10 11.28 5.87 
NavD_061406 06 | d061406 06 -1.05 45.52 45.53 6.82 
-0.02 4.17 4.17 1.92 
5.36 12.83 13.91 7.65 
1.85 8.62 8.81 4.81 
0.73 0.58 0.93 0.42 
-5.19 -10.23 11.47 5.98 
0.76 -17.55 17.57 8.24 
Totals 30.22 179.54 275.10 108.19 
Means 1.78 10.56 
StDev 5.19 15.76 10.90 3.73 
6/15/2006 | NavD_061506 03 | d061506_04 3.88 18.08 18.49 2.92 
NavD_061506 04 | d061506 05 1.67 -37.58 37.62 6.21 
Totals 5.55 -19.50 56.11 9.13 
Means 2.78 -9.75 
StDev 1.56 39.36 13.53 2.33 

















Table 3. ARIES Data with Gyro Rate Bias Set to Zero 
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Date NavD File d File Delta X Delta Y Error (m) | Error % Dist 
Traveled 
7/19/2006 | NavD_071906_03 | d071906_ 03 5.07 2.47 5.63 0.70 
-6.10 -5.58 8.27 1.24 
NavD_071906_ 06 | d071906 07 3.81 1.29 4.02 0.70 
1.52 -3.56 3.87 0.74 
Totals 4.29 -5.39 21.79 3.39 
Means 1.07 -1.35 
StDev 5.01 3.84 2.04 0.26 
7/25/2006 | NavD 072506 01 | d072506 01 2.56 -3.36 4.22 0.42 
NavD_072506 02 | d072506 02 5.16 -0.57 5.19 0.76 
NavD_072506_03 | d072506_03 23.04 -7.62 24.27 1.13 
Totals 30.75 -11.54 33.67 2.30 
Means 10.25 -3.85 
StDev 11.15 3.55 11.30 0.36 
Table 4. ARIES Data with IMU Data Averaging Change 
Date NavD File d File Delta X Delta Y Error (m)_ | Error % Dist 
Traveled 
8/24/2006 | NavD 082406 05 | d082406 05 0.71 -5.32 5.36 0.23 
NavD_082406 06a |d082406 06a 13.96 2.35 14.16 0.33 
Totals 14.68 -2.96 19.52 0.56 
Means 7.34 -1.48 
StDev 9.37 5.42 6.22 0.07 
9/12/2006 | NavD_091206 01 | d091206 02 3.18 9.61 





Table 5. 
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ARIES Data with Pseudo-Adaptive Algorithm 








APPENDIX B: SIMULATION EKF CODE 


The following MATLAB® code was used for running simulations on the data 
sets. The original code was developed by A. Healey with modifications made by H.S. 
Kim and S. Vonheeder during the course of this work. The codes contained in 
Appendices B and C are intended to run in MATLAB® with a specific data set structure 
from the ARIES vehicle. These data sets may be obtained from the NPS Center for AUV 


Research or the codes can be tailored to run with specific data sets to provide the correct 


state information. 








SEEEEEEE EE EE ES EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EES EES EE EE EES EE EE EE EE ES ESESS 
% AUV Research Center 

% Extended Kalman Filter For ARIES AUV 

% Edited: 11/9/06 

SEEEEEEE ES EE ES EE EE EE EE EE EE EE EE EE EE EES EE EES EE EEEEEEEESEEEESEEEE EE EES ES ES EEESS 





% generate default 
defDateFields = 








t£ ( 
ag ey 
t£ ( 


defYear = sprin 
defMon sprin 
defDay sprin 
defDateStamp 
defNameD [d* 








| SEEK © Faerie 
fields 
boxTitle = 
lineNo 1; 
defaultInput 


nam 








"Get Data 


'"S02d',defDateFields 
602d',defDateFields 
'"S02d',defDateFiel 
[defMon defDay defYear 
defDateStamp]; 


i 





from dialog box 
{'D File:'}; 


File'; 


{defNameD}; 








input inputdlg (fiel 
if ( isempty(input) ) 


ds, boxTitle, lineNo, defaultInput, 





disp('No data fil 

return; 
else 

dFileName 
end; 


fe) 


fields = 
boxTitle = 
lineNo 1; 
defNameNavD 
defaultInput 


input 


{'NavD File: 








inputdlg(fiel 


['NavD_' 
{defNameNavD}; 





le selected. Exit program.'); 


input {1}; 


% get NavD-file name from dialog box 


1 


"Get Nav Data'; 


dFileName (2:10) '.d']; 


ds, boxTitle, lineNo, 





defaultInput, 





if ( isempty(input) ) 
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Load data from Desired Run with Aries using d and NavD Files 
file name for dialog box 
datevec (dat 


"on'); 


disp('No data file selected. 








return; 
else 
dFileNameNav = input{1}; 
end; 
d =load(dFileName) ; 
Nav =load(dFileNameNav) ; 


6% Data Assignment to variables 


sss =Nav(1,2); 


eee =length (Nav(:,2))+Nav(1,2)-1; 
mmm =max(size(Nav)); 


startSample=1; 




















endSample=mmm; 

d_khs(:,1)=Nav(:,21); % gpsStatus check !-> ok) 
d_khs(:,2)=Nav(:,32); % IMU theta [rad] 
d_khs(:,3)=Nav(:,31); % IMU phi [rad] 
d_khs(:,4)=Nav(:,54); % IMU psi [rad] 
d_khs(:,5)=Nav(:,8); % yaw_rate [rad/sec] 
d_khs(:,6)=Nav(:,10); % u [m/sec] 

d_khs(:,7)=Nav(:,11); % v [m/sec] 

d_khs(:,8)=Nav(:,12); % w [m/sec] 

d_khs (:,9)=Nav(:,47); % GPS longitude 
d_khs(:,10)=Nav(:,46); & GPS latitude 
d_khs(:,11)=Nav(:,35); % Nav_lat (X [m]) 
d_khs(:,12)=Nav(:,36); % Nav_long (Y [m]) 
d_khs(:,13)=Nav(:,37); % Nav_heading [rad] 

d_khs (:,14)=Nav(:,38); % Nav_yaw_rate [rad/sec] 
d_khs(:,15)=Nav(:,39); % Nav_u [m/sec] 
d_khs(:,16)=Nav(:,40); % Nav_v [m/sec] 
d_khs(:,17)=Nav(:,42); % Nav_Bias_r [rad/sec] (experimentally 
% estimated Bias_psi) 

d_khs(:,18)=Nav(:,41); % Nav_Bias_psi [rad] (experimentally 
% estimated Bias_r) 

Bias_psi =Nav(:,41); % [rad] (experimentally estimated 
Bias_psi) 

Bias_r =Nav(:,42); % [rad/sec] (experimentally estimated 
Bias_r) 

gpsStatus =d_khs(:,1); 

pitch =d_khs(:,2); 

roll =d_khs(:,3); 

heading =d_khs(:,4); 

yaw_rate =d_khs(:,5); 

ug =d_khs(:,6); 

vg_RDI =d_khs(:,7); 

vg d_khs(:,7)-1.0*d_khs(:,5); 




















Exit program. 

















wg =d_khs(:,8); 

long =d_khs(:,9); 

lat =d_khs(:,10); 

NsV =Nav(:,23); 
ll=long(startSample) ; 

12=lat (startSample) ; 
long=long-1ll*ones(length(lat),1); 


la 


fo 








dt 
t= 














t =lat-12*ones (1 





r i=1:mmm, 





ls 


end 








=1/8; 





(gpsStatus ( 
long(i,1)= 











)==0) 


i, 
0. 


2 
ie) 
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n 
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nn 
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O:dt: (Length (ug) -1) *dt; 


1 
0; lat(i,1)= 


ngth(lat),1); 


nheading = zeros(1,length (heading) ); 
nheading(1) = heading(1); 


for i=2:length (heading) 











if abs (heading (i) 























- heading(i-1)) <= pi 





nheading(i) = nheading(i-1) + heading(i) - heading(i-1); 
end 
if heading(i) - heading(i-1) > pi 

nheading(i) = nheading(i-1) + heading(i) - heading(i-1) - 2*pi; 
end 
if heading(i-1) - heading(i) > pi 

nheading(i) = nheading(i-1) + heading(i) - heading(i-1) + 2*pi; 

end 
d 
ading = nheading',; 
MEASUREMENT VECTOR 
[ug, vg, heading, yaw_rate,lat,long]; % Complete measured data 











STATE VECTOR 
x(:,S)=[lat(s),long(s),psi0, yaw_rate(s) *pi/180,ug,vg,br,bpsi]'; 
psi0=heading(startSample) ; 
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% In 


s=st 
Susy 


Bias_psi 


fe) 


6 In 


it 





itializ 


artSample; 


th 
x=zeros (8,endSample) ; 


stat 


vector, 
err=zeros (6,endSample) ; 


x is 8, 


y is 6 


s)=[lat (s),long(s),psi0, yaw_rate(s),ug(s),vg(s),Bias_r(s),... 


(s)]'; 


itial A matrix 
A=zeros (8,8); 





X=x(1,s) 


7; Y=x(2,S);PSi=x(3,Ss) 
br=x (7,8) ;bpsi=x (8,8); 


7c=x (4,8); 


dop_ug=x (5,s) ;dop_vg=x(6,8s); 


A(1,3)=-dop_ug*sin (psi) -dop_vg*cos (psi) ; 


rao) 


=cos (psi); 

=-sSin(psi); 
=dop_ug*cos (psi) -dop_vg*sin (psi) ; 
in(psi); 


less than 4 satellites 
C=zeros (6,8); 


else 





C(1,5)=1; 
C(2,6)=1; 
C(3,3)=1;C(3,8)=1; 
C(4,4)=1;C (4,7) =0; % 
C(5,1)=1; 
C(6,2)=1; 
C=zeros (6, 8); 
C(1,5)=1; 
C(2,6)=1; 
C(3,3)=1;C(3,8)=1; 
C(4,4)=1;C(4,7)=0; 
C(5,1)=0; 
C(6,2)=0; 


20 

Pe OO kar 

els 

“OAS 

.O1; 
-0000001; 
Oss 


Svariance 
Svariance 
Svariance 
Svariance 
Svariance 
Svariance 


(gpsStatus (1,1)==1.0 & NsV(1)>3) % 


C (4,7) 


on 
on 
on 
on 
on 
on 


m*2 
Y, m*2 
psi, rad*2 
r, rad/s)%*2 
ug, (m/s) *2 
vg, (m/s) *2 


xX, 


Q_dummy=[q1;q2;q3;q4;q5;q6;q7; 98]; 
Q=diag (Q_dummy) ; 


nu 
nu 
nu 
nu 
nu 
nu 





No WNE 
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The NsV>3 rejects fixes that are 


Forces B_r to 0 


ol? 


6 Initial CoVariance Matrix 
Note, old_after means measured data at old time, new_before means 
model predicted value 








ale 





ol? 





p_old_after=eye (8) *le-2; 
delx_old_after=zeros (8,1); 

g=ones (8,1); 
psave=zeros (8, startSample:endSample) ; 








6% Main Calculation Loop 
for i=startSample:endSample-1 


ol? 


Measurement Noise - Adjusts the values of the R-matrix based 
% on the quality of the fix or if no fix present. 


H 
my 
2 
n 
< 

lob 


~‘e 


Vv 





ODOnRrFRPRrRFA~ OO ] 
~ 


Se eit 
~. 








end 


R_dummy=[nul;nu2;nu3;nu4;nu5;nu6]; 
R=diag (R_dummy) ; 


% Compute linearized PHI matrix using updated A 
6 Phi=expm(A*dt) ; 
phi=eye (8,8) +A*dt+ (A*dt) *2/2+ (A*dt) *3/6; 
x_old_after=x(:,1i); Sreset initial state 
%[x_new_before]=prop8 (x_old_after,0,0,dt); % nonlinear state 
% propagation 
xold=x_old_after; 





SX=xold(1);Y=xold(2);psi=xold(3);r=xold(4) ;ug=xold(5) ;vg=xold(6) ; 
Sbu=xold(7);bpsi=xold (8); 














fFl=xold(5) *cos (xold(3))-xold(6) *sin(xold(3)); 
fF2=xold(5)*sin(xold(3))+xold(6) *cos(xold(3)); 
fF3=xold(4); 





fF4=0; %Srdot=0; 
F5=0; Sugdot=0; 
F6=0; Svgdot=0; 
f= (£13 £2 3-435 £65070 
xnew=xoldtf.*dt; %Sxd=f; 

x_new_before=xnew; 

p_new_before=phi*p_old_after*phi' + Q; % error covariance 
propagation 








ol? 


ol? 


new gain calculation using linearized new C matrix and current state 
error covariances. 
formulate the innovation using nonlinear output propagation as 
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ae 


ole 


fe) 


% compared to new sampled data from measurements 


if (gpsStatus(it+1,1)==1.0 & NsV(it+1)>3) 
ny_sub=6; 
xold=x_new_before; 





yhat=[xold(5);xold(6);xold(3)+xold(8) ;xold(4)+xold(7);xold(1);xold(2)]; 
else 

ny_sub=4; 

xold=x_new_before; 

yhat=[xold(5);xold(6);xold(3)+xold(8) ;xold(4)+xold(7);0;0]; 
end 





err(:,itl)=(y(itl,1:6)' - yhat); % Innovation 





if (gpsStatus(i+1,1)==1.0 & NsV(i+l,1)>3) 
C=zeros (6,8); 





C(1,9)F1; 
C(2,6)=1; 
C(3,3)=1;C(3,8)=1; 
C(4,4)=1;C(4,7)=0; 
Co, yal; 
C(6,2)=1; 
else 

C=zeros (6,8); 
C(1,5)=1; 
C(2,6)=1; 
C(3,3)=1;C(3,8)=1; 
C(4,4)=1;C(4,7)=0; 
C(5,1)=0; 

C (6,2) =0; 





























if 
((y (14+1,1)==y (1,1)) 11 (y (2+1,1)==0.0) | | (abs (y (it1,1))>=5.0)),C(1,:)= 
0.0*g';end; 

dst 
((y (i+1,2)==y (1,2)) 11 (y (4+1,2)==0.0) || (abs (y (1+1,2))>=5.0)),C(2,:)= 
0.0*g';end; 

if (abs (y(it+1,3)-y(i,3))<=0.000001),C(3,:)=0.0*g';end; 

if (abs (y (itl, 4)-y(i,4))<=0.000001),C(4,:)=0.0*g';end; 

if (abs (y(it+1,5)-y(i,5))<=0.000001),C(5,:)=0.0*g';end; 

if (abs (y(it+l1,6)-y(i,6))<=0.000001),C(6,:)=0.0*g';end; 
else 

it 
((y (i+1,1)==y (1,1)) 11 (y (i+1,1)==0.0) 1] (abs (y (44+1,1))>=5.0)),C(1,:)= 
0.0*g';end; 

dds. 
((y (i+1,2)==y (1,2)) 11 (y (4+1,2)==0.0) | | (abs (y (1+1,2)) >=5.0)),C(2,:) =... 
0.0*g';end; 

if (abs (y(it+1,3)-y(i,3))<=0.000001),C(3,:)=0.0*g';end; 

if (abs(y(it+l1,4)-y(i,4) )<=0.000001),C(4,:)=0.0*g';end; 
end 
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% Compute gain, update Total State and error covariance 
G=p_new_before*C(1:ny_sub,:) '*inv(C(l:ny_sub,:) *p_new_before%... 
C 


fo) 


(l:ny_sub,:)' + R(1l:ny_sub,1l:ny_sub)); % Kalman Gain 





p_temp=G*C (1l:ny_sub,:) *p_new_before; 
p_old_after=p_new_before-p_temp; 





psave(:,it1l)=diag(p_old_after) ; 
x_new_after=x_new_befor + G*err(l:iny_sub,itl); 





6a 
ol 


sendSample 


scarry new state into next update 
x(:,itl)=x_new_after; 





sresetting up the linearized A matrix 
A=zeros (8,8); 
X=x (1,i+1);Y=x(2,it+1);psi=x(3,i+1);r=x(4,1i+1); 

dop_ug=x (5, i+1) ;dop_vg=x (6,i+1);bu=x(7,i+1);bpsi=x(8,it1); 





























A(1,3)=-dop_ug*sin (psi) -dop_vg*cos (psi) ; 
A(1,5)=cos (psi); 

A(1,6)=-sin (psi); 
A(2,3)=dop_ug*cos (psi) -dop_vg*sin (psi) ; 
A(2,5) 

A(2,6) 

A(3,4) 


6% Plot Results in Figures 
%& figure(l1) 


plot (long (startSample:endSample),lat (startSample:endSample),'r+'),grid 


























% hold on 

% plot (x(2,startSample:endSample),x(1,startSample:endSample),'g.') 
% hold off 

% title('Filter Estimated Path(green), GPS (red) ') 

% ylabel('latitude in meters') 

% xlabel('longitude in meters') 

% grid 

% axis equal 


figure(2) % Path Plot 














Splot(long,lat,'r+'); 

Shold on 

plot (x(2,:),x(1,:),'9g-'"); 

hold on 

plot (d_khs(:,12)-d_khs(s,12),d_khs(:,11)-d_khs(s,11),'b-'); 
plot (GPS_3(:,2)-d_khs(s,12),GPS_3(:,1)-d_khs(s,11),'*k',... 


GPS_4(:,2)-d_khs(s,12),GPS_4(:,1)-d_khs(s,11),'*m',... 
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GPS_5(:,2)-d_khs(s,12),GPS_5(:,1)-d_khs(s,11),'*r"'); 
hold off 
grid 
title('POSITION: GPS, Filter Estimated, In-Vehicle Estimated'); 
legend('Filter', 'Vehicle Filter','GPS NsV=3','GPS NsV=4"',... 

"GPS NsV=5'); 
ylabel('X [m]') 
xlabel('Y [m]"') 
axis equal 


























% X Dimension 
(startSample:endSample),lat(startSample:endSample),'r+',... 
( 

( 


ol? 


figure (3) 
plot ( 


ol? 








le 





t 
12 
i 


ol? 


startSample:endSample),d_khs (startSample:endSample, 11)- 
a_khs (sz7 11) "be")$ 

title('X: measured [red], sim estimated [green], exp estimated 
[blue]'); 





ol? 


ole 





ae ole 


ol? 


figure(4) % Y Dimension 


ol? 


ol? 


plot (t (startSample:endSample),long(startSample:endSample),'r+',... 


ole 








ae 


t (startSample:endSample) ,d_khs (startSample:endSample, 12) — 
d_khs(s,12),'b.'); 
title('Y: measured [red], sim estimated [green], exp estimated 
[blue]'); 


ol? 





ol? 


ol? 


figure(5) % Heading Results 


plot (t (startSample:endSample) , heading (startSample:endSample),'r+',... 


startSample:endSample),x(1,startSample:endSample),'g.',... 


t(startSample:endSample),x(2,startSample:endSample),'g.',... 


t(startSample:endSample), (x(3,startSample:endSample)),'g.',... 


t(startSample:endSample), (d_khs (startSample:endSample,13)),'b.'); 
title('Heading (radians): measured [red], sim estimated [green],... 
exp estimated [blue]'); 


fe) 


figure(6) % Yaw Rate 


plot (t (startSample:endSample),yaw_rate(startSample:endSample),'rt+',... 


t(startSample:endSample),x(4,startSample:endSample),'g.', 


t(startSample:endSample),d_khs (startSample:endSample,14),'"b.'); 
title('Yaw_rate: measured [red], sim estimated [green], exp... 
estimated [blue]'); 








figure(7) % Speed, ug 
plot (t (startSample:endSample),ug(startSample:endSample),'rt+',... 
t(startSample:endSample),x(5,startSample:endSample),'g.',... 








t(startSample:endSample),d_khs (startSample:endSample,15),'b.'); 
title('ug: measured [red], sim estimated [green], exp estimated... 
[blue]'); 





fe) 


figure(8) % Speed, vg 
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plot (t (startSample:endSample),vg_RDI(startSample:endSample),'r+', 
t(startSample:endSample),x(6,startSample:endSample),'g.',... 


t(startSample:endSample),d_khs (startSample:endSample,16),'b.'); 
title('vg: measured [red], sim estimated [green], exp estimated... 
[blue]'); 





figure(9) % Bias_r 
plot (t (startSample:endSample),x(7,startSample:endSample),'g.',... 
t(startSample:endSample),Bias_r(startSample:endSample,1),'b. 














grid 

title('Bias_r: sim estimated [green], exp estimated [blue]'); 
xlabel('time in seconds") 

ylabel ('[deg/sec]') 








figure(10) % Bias_psi 
plot (t (startSample:endSample),x(8,startSample:endSample),'g.',... 





t(startSample:endSample) ,Bias_psi(startSample:endSample,1),'b.') 





grid 
title('Bias_\psi: sim estimated [green], exp estimated [blue]'); 
xlabel('time in seconds") 














ylabel('B_\psi (rad) ') 
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APPENDIX C: SIMULATION UKF CODE 


The following MATLAB code was used for running simulations on the data sets 
to evaluate UKF performance with EKF performance. The original code was developed 
by H.S. Kim based on Julier and Uhlmann’s algorithm (1997) with modifications made 
by S. Vonheeder during the course of this work. 








SS SESESESEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEESEESEESEESES 
% AUV Research Center % 
% Unscented Kalman Filter For ARIES AUV % 
% Edited: 11/09/06 % 
EESEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE EEE EEE EEEEEEEEEESEESS 








6% Load data from Desired Run with Aries using d and NavD Files 
% generate default file name for dialog box 
defDateFields = datevec(date); 











defYear = sprintf ('%02d',defDateFields(1) - 2000); 
defMon = sprintf ('%S02d',defDateFields (2) ); 

defDay = sprintf('%02d',defDateFields(3)); 
defDateStamp = [defMon defDay defYear '_01.d']; 


defNameD = ['d' defDateStamp]; 











% get d-file name from dialog box 
fields = {'D File:'}; 
boxTitle = 'Get Data File'; 
lineNo = 1; 


defaultInput = {defNameD}; 
input = inputdlg(fields, boxTitle, lineNo, defaultInput, 'on'); 
if ( isempty(input) ) 














disp('No data file selected. Exit program.'); 
return; 

else 
dFileName = input{1l}; 

end; 


fe) 


% get NavD-file name from dialog box 
fields = {'NavD File:'}; 





boxTitle = 'Get Nav Data'; 
lineNo = 1; 
defNameNavD = ['NavD_' dFileName(2:10) '.d']; 


defaultInput = {defNameNavD}; 
input = inputdlg(fields, boxTitle, lineNo, defaultInput, 'on'); 
if ( isempty(input) ) 














disp('No data file selected. Exit program.'); 
return; 

else 
dFileNameNav = input{1}; 

end; 
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d =load(dFileName) ; 
Nav =load(dFileNameNav) ; 











20 
CO 





& Data Manipulation from the experiment NAV data 








mmm=max (size (Nav) ); 
startSample=1; 


























endSample=mmm; 

d_khs(:,1) =Nav(:,21); % gpsStatus check !-> ok) 
d_khs(:,2) =Nav(:,32); % IMU theta [rad] 
d_khs(:,3) =Nav(:,31); % IMU phi [rad] 
d_khs(:,4) =Nav(:,54); % IMU psi [rad] 
d_khs(:,5) =Nav(:,8); % yaw_rate [rad/sec] 
d_khs(:,6) =Nav(:,10); % u [m/sec] 

d_khs(:,7) =Nav(:,11); % v [m/sec] 

d_khs(:,8) =Nav(:,12); % w [m/sec] 

d_khs(:,9) =Nav(:,47); % GPS longitude 
d_khs(:,10)=Nav(:,46); & GPS latitude 
d_khs(:,11)=Nav(:,35); % Nav_lat (X [m]) 
d_khs(:,12)=Nav(:,36); % Nav_long (Y [m]) 
d_khs(:,13)=Nav(:,37); % Nav_heading [rad] 
d_khs(:,14)=Nav(:,38); % Nav_yaw_rate [rad/sec] 
d_khs(:,15)=Nav(:,39); % Nav_u [m/sec] 
d_khs(:,16)=Nav(:,40); % Nav_v [m/sec] 
d_khs(:,17)=Nav(:,42); % Nav_Bias_r [rad/sec] (exp est Bias_psi) 
d_khs(:,18)=Nav(:,41); % Nav_Bias_psi [rad] (exp ested Bias_r) 
Bias_psi Nav(:,41); % [rad] (exp est Bias_psi) 
Bias_r =Nav(:,42); % [rad/sec] (exp est Bias_r) 








% Initialize th xperimental results 














gpsStatus =d_khs(:,1); 
pitch =d_khs(:,2); 
roll =d_khs(:,3); 
heading =d_khs(:,4); 
yaw_rate =d_khs(:,5); 
ug =d_khs(:,6); 
vg =d_khs(:,7)-1.0*d_khs(:,5); 
wg =d_khs(:,8); 
long =d_khs(:,9); 
lat =d_khs(:,10); 





ll=long(startSample) ; 

12=lat (startSample) ; 
long=long-1l1.*ones(length(lat),1); 
lat=lat-12*ones (length(lat),1); 














for i=1:mmm, 
if (gpsStatus ( 
long(i,1)= 


Ly 1 )==0)) 
0.0; lat (i,1)=0.0; 
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t=0:dt: (length (ug) -1) *dt; 








% Heading Signal Wrapping 








nheading = zeros(1, length(heading) ); 
nheading(1) = heading(1); 


for i=2:length (heading) 

















if abs (heading(i) - heading(i-1)) <= pi 

nheading(i) = nheading(i-1) + heading(i) - heading(i-1); 
end 
if heading(i) - heading(i-1) > pi 

nheading(i) = nheading(i-1) + heading(i) - heading(i-1) - 2*pi; 
end 
if heading(i-1) - heading(i) > pi 

nheading(i) = nheading(i-1) + heading(i) - heading(i-1) + 2*pi; 
end 
end 


heading = nheading',; 


























% MEASUREMENT VECTOR 








y=[ug,vg, heading, yaw_rate,lat,long]; scomplete measured data 








%& Initialize the UKF parameters and state vector, x is 8, y is 6 








scale=3; 
kappa=scale-na; 
WO_m=kappa/ (natkappa) ; 
Wi_m=1/ (2* (nat+tkappa) ); 
) 
) 





ia 





WO_c=kappa/ (natkappa 
Wi_c=1/ (2* (nat+tkappa) 





- 


s=startSample; 

x=zeros(8,endSample); err=zeros(6,endSample) ; 
psi0=heading(s); 
x(:,S)=[lat(s),long(s),psi0,yaw_rate(s),ug(s),vg(s),... 
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Bias_r(s),Bias_psi(s)]'; 


xa(:,S)=[x(:,8)]; 
xa_old=xa(:,s); 








%% System Noise 

ql=0.01; Svariance on X, m*2 Cannot be 0, Q pos semi-def 
q2=0.01; Svariance on Y, m*2 for Cholesky Decomposition 
q3=0.001; Svariance on psi, rad%*2 

q4=0.1; Svariance on r, (rad/s)%*2 

q5=0.01; $variance on ug, (m/s) %*2 

qé6=0.01; $variance on vg, (m/s) %*2 

q7=0.0000001; Svariance on Br 

q8=0.0 $variance on B_psi 


Q_dummy=[q1;q2;q3;q4;q5;q6;q7; 98]; 
Q=diag (Q_dummy) ; 


fe) 


% Measurement Noise 


nul=0.1; 

nu2=0.1; 

nu3=0.001; 

nu4=0.001; 

nu5=1.0; 

nu6=1.0; 
R_dummy=[nul;nu2;nu3;nu4;nu5;nu6j; 
R=diag (R_dummy) ; 








ol? 


% 





ol? 


Initial Pa matrix 





oe 





p_old_after=eye (nx) *le-2; 
delx_old_after=zeros (nx,1)/; 
g=ones (nx,1); 


psave=zeros (nx, startSample:endSample) ; 


Pa_old=p_old_after; 








6% Main Calculation Loop 
for i=startSample:endSample-1 
SL 


sendSample 


o\ 





ol? 





ole 


1. Calculate Sigma points 





oe 





bias_Pa=chol ((nat+tkappa) *Pa_old); 








ole 


Sigma_points_old(:,1)=xa_old; 


for j=l:na, 
Sigma_points_old(:,j+1)=xa_oldtbias_Pa(j,:)'; 
Sigma_points_old(:,na+j+1)=xa_old-bias_Pa(j,:)'; 





end 


pee 





ol? 





ol? 


Time Update 





ol? 





ole 


Des 


ole 


Nonlinear state propagation 





for j=1: (2*nat+1) 
% Sigma_points_new_before (1 
(Sigma_points_ol 


ol? 





ld(l:nx,j), (process_noise*0.0) 


l:nx, j)=prop_ukf... 














xold=Sigma_points_old(1l:nx,j); 

Fl=xold(5) *cos (xold(3))-xold(6)*sin(xold(3)); 
F2=xold(5)*sin(xold(3))+xold(6) *cos (xold(3)); 
fF3=xold(4); 

fF4=0; %Srdot=0; 

F5=0; Sugdot=0; 

F6=0; Svgdot=0; 

f£7=0; SBias_r_dot=0; 

F8=0; SBias_psi_dot=0; 
F=(flet2atser4e toero sty. £384)4 





xnew=xoldtf.*dt; 


Sigma_points_new_before (1:nx,j)=xnew; 


,dt); 





mean value of x_new_before 





mean_x_new_before=zeros (nx,1); 

















for j=1: (2*nat1) 
if j== 
mean_x_new_before=mean_x_new_before+W0O_m*... 
Sigma_points_new_before(l:nx,}j); 
else 
mean_x_new_before=mean_x_new_beforet+Wi_m*... 
Sigma_points_new_before(1l:nx,}j); 
end 
mean_x_new_before(7)=0; % B_r=0 
end 
% 4. error covariance propagation 
P_new_before=Q; 
for j=1:(2*natl), 


if j== 





P_new_before= 
(Sigma_poi 
(Sigma_poi 

else 


P_new_beforet+W0_c*... 
nts_new_before (l:nx, j) -mean_x_new_before)*... 
nts_new_before(1:nx, j) -mean_x_new_before) '; 





P_new_before= 
(Sigma_poi 
(Sigma_poi 


P_new_beforet+Wi_c*... 
nts_new_before(l:nx, J) -mean_x_new_before)*... 
nts_new_before(1:nx, j) -mean_x_new_before) '; 











end 
end 
%$ 5. output Sigma_yhat 
for j=1:(2*natl), 
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if gpsStatus (it+1)== 
ny_sub=ny; 
% Sigma_yhat_before(l:iny,j)=output_ukf_w_GPS... 
% (Sigma_points_new_before(1:nx, 4), (measurement_noise*0.0)); 
xold(1l:nx)=Sigma_points_new_before(1l:nx, J); 








1d ( 
y2=xold(6); 
y3=xold(3)+xold(8); 
y4=xold(4)+xold(7) *0; 
y5=xold(1); 





yhat=[yliy2;y3;y4;y5;yel; 
Sigma_yhat_before(l:ny,j)=yhat; 
else 


% Sigma_yhat_before(l:ny, j)=output_ukf_wo_GPS... 
% (Sigma_points_new_before(1:nx, 4), (measurement_noise*0.0)); 








xold(l:nx)=Sigma_points_new_before(1l:nx,]j); 
yl=xold(5); 

y2=xold(6); 

y3=xold(3)+xold(8); 

y4=xold(4)+xold(7) *0; 

y5=xold(1); 

y6=xold(2); 

yhat=[yliy2;y3;y4;0;0]; 





Sigma_yhat_before(l:ny, j)=yhat; 
end 
end 





ol? 


oe 


6. mean value of yhat 


ole 





mean_yhat=zeros (ny,1l); 
for j=1:(2*natl), 


mean_yhat=mean_yhat+W0_m*Sigma_yhat_before(li:iny,j); 
else 





mean_yhat=mean_yhat+Wi_m*Sigma_yhat_before(li:iny, Jj); 
end 
end 





ol? 





ole 


Measurement Update 





ole 








ole 


oe 


1. Pyy_bar and Pxy 





ol? 


Pyy_bar=zeros (ny_sub,ny_sub) ; 
Pxy=zeros (nx,ny_sub); 


for j=1:(2*natl), 
err_x=Sigma_points_new_before (1:nx, j]) -mean_x_new_before; 
err_y=Sigma_yhat_before(l:ny_sub, j) -mean_yhat (1l:ny_sub,1); 








Pxy=Pxyt+W0O_c*err_x*err_y'; 
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Pyy_bar=Pyy_bar 
else 


WO_c*err_y*err_y'; 











Pxy=Pxyt+Wi_c*err_x*err_y'; 
Pyy_bar=Pyy_bar+Wi_c*err_y*err_y'; 









































end 
end 
S_temp=Pyy_bar-R(1l:ny_sub,1:ny_sub); 
if gpsStatus (i+1)==1 
if ((y(it1,1)==y(i,1)) 11 (y (i+1,1)==0.0) 11]... 

(abs (y (it1,1))>=5.0)),Pxy(:,1)=0.0%*g; 
S_temp(:,1)=0.0*g(1l:ny_sub,1); 
S_temp(1,:)=0.0*g(l:ny_sub,1)'; 

end 
if ((y(i+1,2)==y(i,2)) | 1 (y (i+1,2)==0.0) |]... 

(abs (y(it1,2))>=5.0)),Pxy(:,2)=0.0*g; 

S_temp(:,2)=0.0*g(1l:ny_sub,1); 
S_temp (2,:)=0.0*g(1l:ny_sub,1)'; 

end 

if (abs (y(it1,3)-y(i,3))<=0.000001),Pxy(:,3)=0.0%*g; 
S_temp(:,3)=0.0*g(1l:ny_sub,1); 
S_temp (3,:)=0.0*g(1l:ny_sub,1)'; 

end 

if (abs (y(it+l1,4)-y(i,4))<=0.000001),Pxy(:,4)=0.0%*g; 
S_temp (:,4)=0.0*g(1:ny_sub,1); 
S_temp (4,:)=0.0*g(1l:ny_sub,1)'; 

end 

if (abs (y(it1,5)-y(i,5))<=0.000001) ,Pxy(:,5)=0.0*g; 
S_temp(:,5)=0.0*g(1l:ny_sub,1); 
S_temp (5,:)=0.0*g(1l:ny_sub,1)'; 

end 

if (abs (y(itl,6)-y(i,6))<=0.000001) ,Pxy(:,6)=0.0%*g; 
S_temp(:,6)=0.0*g(1l:ny_sub,1); 
S_temp (6,:)=0.0*g(1l:ny_sub,1)'; 

end 

else 

if ((y(it1,1)==y(i,1)) 11 (y (i+1,1)==0.0) 1]... 

(abs (y(it1,1))>=5.0)),Pxy(:,1)=0.0%*g; 

S_temp(:,1)=0.0*g(1l:ny_sub,1); 
S_temp (1,:)=0.0*g(1l:ny_sub,1)'; 
end; 
if ((y(it+1,2)==y(i,2)) | 1 (y (i+1,2)==0.0) |]... 

(abs (y(it1,2))>=5.0)),Pxy(:,2)=0.0%*g; 
S_temp(:,2)=0.0*g(1l:ny_sub,1); 
S_temp (2, :)=0.0*g(1l:ny_sub,1)'; 

end; 

if (abs (y(it1,3)-y(i,3))<=0.000001),Pxy(:,3)=0.0*g; 
S_temp (:,3)=0.0*g(1:ny_sub,1); 
S_temp (3,:)=0.0*g(1l:ny_sub,1)'; 

end; 

if (abs (y (itl, 4)-y(i,4))<=0.000001),Pxy(:,4)=0.0%*g; 
S_temp (:,4)=0.0*g(1:ny_sub,1); 
S_temp (4, :)=0.0*g(1l:ny_sub,1)'; 

end; 


’ 
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% 2. UKF Gain matrix 


5 





UKF_K=Pxy*inv(S_temp+R(1l:ny_sub,1:ny_sub)); 











err(:,i+1)=(y(itl,:) '-mean_yhat) ; 





if sqrt (err(5,i+1)*2+err(6,it+1)%*2)>100, 
err (5,it+1)=0;err(6,i+1)=0; 
end; 





_new=mean_x_new_before+UKF_K*err(l:ny_sub,it+1l); 
_new(7)=0; 





4. P_new 








P_new=P_new_before-UKF_K* (Pyy_bar) *UKF_K'; 





6 5. xa_old & Pa_old 





xa_old=x_new; 
Pa_old=P_new; 
psave(:,it1)=diag(P_new) ; 
x(:,it+1)=x_new; 

end 





6% Plot Results in Figures 
figure(1),clf 


plot (long (startSample:endSample),lat (startSample:endSample),'r.') 
grid 

hold on 

plot (x(2,startSample:endSample),x(1,startSample:endSample),'g-') 
hold off 








title('Filter Estimated Path(green),GPS (red) ') 




















ylabel('latitude in meters') 
xlabel('longitude in meters') 
grid 


axis equal 


figure(2), clf % Path Plot 

plot (long,lat,'r.'); 

hold on 

plot (x (2,2) ,*(1, 2) 4 '9=")7 

plot (d_khs(:,12)-d_khs(s,12),d_khs(:,11)-d_khs(s,11),'b-'); 
hold off 

grid 

legend('GPS Data', 'UKF Estimate', 'EKF Estimate'); 

title (strcat (dFileName(1:7),'\_',dFileName(9:12),' Path Plot')); 
% title('POSTION: GPS [red], UKF Estimated [green] , In-Vehicle 
EKF Estimated [blue]'); 





























ol? 
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ylabel('X [m]') 
xlabel('Y [m]') 
axis equal 














figure(3), clf % X Dimension 
plot (t (startSample:endSample),lat(startSample:endSample),'r.',... 
t(startSample:endSample),x(1,startSample:endSample),'g-',... 
t(startSample:endSample),d_khs (startSample:endSample, 11)... 
-d_khs(s,11),'b-'); 
title('X: measured [red], sim estimated [green],... 
exp estimated [blue]'); 














figure(4), clf % Y Dimension 
plot (t (startSample:endSample),long(startSample:endSample),'r.',... 
t(startSample:endSample),x(2,startSample:endSample),'g-',... 
t(startSample:endSample),d_khs (startSample:endSample, 12)... 
-d_khs(s;12) 4 'b-')3 
title('Y: measured [red], sim estimated [green],... 
exp estimated [blue]'); 














figure(5), clf % Heading Results 

plot (t (startSample:endSample) , heading (startSample:endSample),'r.',... 
t(startSample:endSample), (x(3,startSample:endSample)),'g-',... 
t(startSample:endSample), (d_khs (startSample:endSample,13)),'b- 











'); 
title('Heading (radians): measured [red], sim estimated [green], 
exp estimated [blue]'); 


figure(6), clf % Yaw Rate 

plot (t (startSample:endSample),yaw_rate(startSample:endSample),'r.',... 
t(startSample:endSample),x(4,startSample:endSample),'g-',... 
t(startSample:endSample),d_khs (startSample:endSample, 14) ,... 








"b-")7 
title('Yaw rate: measured [red], sim estimated [green],... 
exp estimated [blue]'); 


figure(7), clf % Speed, ug 
plot (t (startSample:endSample),ug(startSample:endSample),'r.',... 
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t(startSample:endSample),x(5,startSample:endSample),'g.',... 











t(startSample:endSample),d_khs (startSample:endSample,15),'b.'); 
title('ug: measured [red], sim estimated [green],... 
exp estimated [blue]'); 


figure(8), clf % Speed, vg 
plot (t (startSample:endSample),vg(startSample:endSample),'r.',... 
t(startSample:endSample),x(6,startSample:endSample),'g.',... 
t(startSample:endSample),d_khs (startSample:endSample,16),'b."'); 
title('vg: measured [red], sim estimated [green],... 
exp estimated [blue]'); 














figure(9), clf % Bias_r 
plot (t (startSample:endSample),x(7,startSample:endSample),'g.',... 
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t(startSample:endSample),Bias_r(startSample:endSample,1),'b. 








grid 

title('Bias_r: sim estimated [green], exp estimated [blue]'); 
xlabel('time in seconds") 

ylabel ('[deg/sec]') 








figure(10), clf % Bias_psi 
plot (t (startSample:endSample),x(8,startSample:endSample),'g.',... 
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t(startSample:endSample) ,Bias_psi(startSample:endSample,1),'b.') 








grid 
title('Bias_\psi: sim estimated [green], exp estimated [blue]'); 
xlabel('time in seconds") 








ylabel('B_\psi (rad) ') 
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ee 


APPENDIX D: VEHICLE IMU CODE 


The following provides the IMU code, written in C, that processes the IMU data 
output and performs the necessary reference frame rotations as well as the integration of 
the yaw rate for the heading reference. The accelerations, angular rates and heading are 
sent to the navigation filter for further processing. The original code was developed by J. 


Nicholson, CAPT, USN and modified by S. Kragelund for this research. 


/* 
** Modifications to Jack Nicholson's IMU.c for realtime (8Hz) operation 
xx This program downsamples the 100Hz IMU data, processing one message 
** each 8Hz interval. Checksum verification has been added to ensur 

** only valid data gets passed to the Nav process 


xk 


** SPK/DTD 07/14/06 











i 








define TRUE 1 
define FALSE 0 








include "IMU.h" 


// SPK 010306: Need this to access migration library 
include <mig4nto.h> 
include <sys/neutrino.h> 
include <sys/netmgr.h> 





// SPK 011106: Added for Neutrino timer pulses 
define MP_PULSE_CODE PULSE_CODE_MINAVAIL 























define IMU_MSG_SIZE 44 
define PITCH_LIMIT (60.0) // degrees 
define ROLL_LIMIT (60.0) // degrees 











// SPK 070506: Define values for IMU status word #1 bits 


























define IMU_ACCEL_TEMP OxFFOO // Bits 15-8 (LSB = 1 deg C) 

define IMU_RLG_A PLC 0x0080 // Bit 7 (a-axis RLG in PLC reset) 
define IMU_RLG_B PLC 0x0040 // Bit 6 (b-axis RLG in PLC reset) 
define IMU_RLG_C_PLC 0x0020 // Bit 5 (c-axis RLG in PLC reset) 

















define IMU_FAIL 
define IMU_SW1_ 
define IMU_COUNTI 


RE 0x0010 // Bit 4 
RR_BITS OxO00FO // Bits 7-4 
ER OXOOOF // Bits 3-0 (4-bit counter) 





FG 











// SPK 070506: Define values for IMU status word #2 bits 
define IMU _PROC_FAILURE 0x8000 // Bit 15 (Processor tests failed) 


define IMU_MEM FAILURE 0x4000 // Bit 14 (Memory tests failed) 
define IMU_OTHER_FAILURE 0x2000 // Bit 13 (Other tests failed) 
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define IMU_ACCEL_FAILURE 0x1000 // Bit 12 (Accelerometer tests 


























define IMU_GYRO_FAILURE 0x0800 // Bit 11 (Gyro tests failed) 
define IMU_RSV1_FAILURE 0x0400 // Bit 10 (Reserved) 

define IMU_RSV2_FAILURE 0x0200 // Bit 9 (Reserved) 

define IMU_RSV3_FAILURE 0x0100 // Bit 8 (Reserved) 

define IMU_SW2_ERR_BITS OxF800 // Bits 15-11 

define IMU_SW_VER_NUMBER Ox0OOFF // Bits 7-0 (Software Version #) 














// SPK 070606: Define value to identify a checksum error 
define IMU_CHKSUM_ERROR Ox0001 



























































FILE *IMUalignfp; 

FILE *NavLatfp; 

FILE *Outfp; // Use during testing 
FILE *DateStampiInfp; // Use during testing 


double pi=3.14159265; 


// This function retrieves the last IMU message in the buffer at time, 
// verifies its checksum, and returns an integer offset for the first 
// byte of the message 
int getLastMessage (buffer, numBytes) 

u_char* buffer; 

int numBytes; 


tnt ave 

int found = FALSE; 

unsigned short chkSum; 
unsigned short computedChkSum; 





if (numBytes < IMU_MSG_SIZI 
return -1; 





Gl 
~~ 
























































for (i = (numBytes —- IMU_MSG_SIZE); ((i >= 0) && (!found)); i--) 
{ 
if ((* (obufferti) == O0xA5) && (* (bufferti+l) == 0x02) ) 
{ 
chkSum = *(unsigned short*) (buffer iL IMU_MSG_SIZE 
2) 
computedChkSum = * (unsigned short*) (buffer 1 2) 
*(unsigned short*) (buffer i 4) 
* (unsigned short*) (buffer 1 6) 
*(unsigned short*) (buffer iL 8) 
*(unsigned short*) (buffer i 10 
* (unsigned short*) (buffer 1 12 
*(unsigned short*) (buffer i 14) 
*(unsigned short*) (buffer i 16 
*(unsigned short*) (buffer i 18 
*(unsigned short*) (buffer i 20 
*(unsigned short*) (buffer a 22 
*(unsigned short*) (buffer 1 24) 
*(unsigned short*) (buffer ap 26 
*(unsigned short*) (buffer i 28 
































80 
































*(unsigned short*) (buffer L 30) 
*(unsigned short*) (buffer a 32) 
*(unsigned short*) (buffer ais 34) 
*(unsigned short*) (buffer a 36) 
*(unsigned short*) (buffer 1 38) 
*(unsigned short*) (buffer i 40); 
if (chkSum == computedChkSum) 
{ 
found = TRUE; 
itt; 
} 
else 
{ 
printf ("IMU.c: getLastMessage: Bad Checksum! \n") ; 
} 
} // if ((*buffertl... 
} // for (i... 
//printf ("Index %d\n", i); 
TE (oa O04 
{ 
printf ("IMU.c: getLastMessage: No Valid Record Found!\n"); 


} 


return i; 


} // getLastMessage 


i,k,] 


IMU_Shmid, IMUFlag_Shmid, HMR_Shmid, read_status; 


_r,i_m,Msg, StartByte; 





Port, 


double g 


// SPK O01 
// 
//value 


// 


BytesRead; 


0306: 


changed from char to unsigned char arrays to prevent 


u_char ReadBuf [2000]; 


//port 


u_char MsgBuf [44]; 


//parsing da 


ta 


char FileCommand[256]; 
char FileString[256]; 





short p, 
int dAx, 
//velocities 


// SPK O07 
unsigned 
unsigned 
unsigned 


ay &, 
dAy, 


0506: 
short 
short 
short 


int LastMsgIdx 


ax, 
dAz, 


ayy 
avx, 


compiler warnings due to comparisons with 165 


(greater than maximum value for signed chars) 
// Buffer for data coming in from serial 
// Message buffer, for aligning and 

latitude... 

inp 

angular rates and accel 

// Inertial delta angles / 


// For reading 
// ...from Nav. 
// Flt control 
avVz; 


az; 
dvy, 


Added to verify IMU message checksums 
int msgCkSum = 0; 


int statusl 
int status2 
=k 


0; 
0; 
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// SPK 062606: added variables for first-order accelerometer filter 















































double Tau, Coefl1,Coef2,phiThresh, thetaThresh; 

int initFilter = FALSE; 

int filterAcc = FALSE; // set to TRUE for first-order filtering 
of accelerometer data 

double p_out, q_out, r_out, ax_out, ay_out, az_out; // 
Variables written... 

double dAx_out, dAy_out, dAz_out,dPsi_out; // 
...to shared memory. 

double theta_out, phi_out; // 
2 Euler angles 

double dt; // 
sampling interval 

double Lat0O, LatD, Lat; // 
GPS Origin latitude 

double s_phi,c_phi,s_theta,c_theta,s_psi,c_psi,s_Lat,c_Lat; // 
Sines and cosines 

double q_e,r_e; // 
Earth rates in q,r 

double psi_out, psi_dot, dPsi; // 
Heading 





// SPK 062606: Default behavior will write to 


unless FILE FLAG = 0 








r 














100 Hz data to a file 
int INIT_ERROR = 0; 











IMU failure 


contains an error 





containing error bits 











of error occurred 


u 
e 
int IMU_Id=0; 
as 
i 





nt IMU_Kil1l=0; 
nt N_Samples=12; // Number of 100Hz samp] 
writing to shared memory 





int FILE_FLAG=FALSE; // <<--— Set FILE_FLAG to 


int IMU_Error=0; // Flag 





unsigned char IMU_msgError = 1; // Flag 


shared memory AND file 


zero to stop writing 


to abort mission upon 


if individual message 





unsigned short int IMU_msgErrBits = 0;// bits 


nsigned short int IMU_ErrorType = 0; // Bits 


LES 





int nSamp = 0; // number of 100Hz samp] 
calculate average 
double Align_theta =0.0; 














alignment... 

double Align_phi = 0.0; 

double C_Rate=pow(2,-20) *600; 
to multiply ... 

double C_Accel=pow(2,-14) *600; 
writing to shared... 

double C_DeltaAng=pow (2,-33); 
by data LSB,... 

double C_DeltaV=pow(2,-27); 








control data. 
double Rotation=0.0; 
initialization 
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LES 


// 


iy 
// 


// 


// 


// 


// 


within status words 


indicating what kind 


to process before 


actually used to 





Default IMU mechanical 


...-angles (radians). 
Coefficients by which 


...Summed data before 
..memory. They scale 
...and average flight 


Integrated yaw rate 





double Omega=2*pi/ (24*60*60); // Barth rate in radians 
per second 

double DegRad = 0.01745329; // Convert degrees to 
radians 





// Open IMU shared memory **** FOR TESTING, OPEN IMUd_mmddyy_nn.d 
DATA FILE 

// SPK 071006: allocate space for null character in s 

char Filename[17],s[3]; 

char ShellCom[] = "date '+IMUd_Sm%d%y .d' > IMUDateStamp"; 














// Real Time Loop Stuff 

int Hz,t_count; 

double t; 

pid_t LoopTimerProxy; 
timer_t LoopTimerld; 

struct itimerspec LoopTimer; 
struct sigevent event; 


// SPK 011106: Added for Neutrino timer pulses 
int timerChId; 

int timerConId; 

int timerRcvlid; 

struct _pulse pulseMsg; 


Hz = 8; 
dt = 1.0 / (double) Hz; 


// SPK 010306: Get I/O privity for this thread in Neutrino 
// (This process must still be run as root) 

// Note: This replaces th Tl compiler option from QNX4 
if ( ThreadCtl ( _NTO_TCTL_IO, NULL ) == -1 ) 

{ 





perror ("I/O Privilege Request failed!\n"); 
return; 


system (Shel1Com) ; 
if (FILE_FLAG) 
{ 





DateStampInfp = fopen("IMUDateStamp","r"); 


for (k=0;k<12;++k) Filename[k] = getce(DateStampInfp) ; 
k=1; 
while(1) 
{ 
if (k<10) 
sprintf(s,"0%d",k); 
else 
sprintf(s,"%d",k); 


Filename[12] = s[0]; 
Filename [13 
Filename [14 
Filename [15 


ms ss EH 
| 
n 
= 
hb 
4 
~ 
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Filename[16] = NULL; 

// SPK 062606: Make this file binary for size/speed reasons 
if ((Outfp=fopen (Filename, "r+b") ) ==NULL) 

{ 











Outfp = fopen(Filename, "wb") ; 
// SPK DEBUG 

if" .( GOubip SS NUGL.) 

{ 





printf ("Cannot open output file!\n"); 
exit (0); 


break; 
} 
else 
{ 
fclose (Outfp) ; 
t++k; 
if (k==100) 
{ 





printf ("Cannot Create File Number 100\n"); 
exit (0); 
} 
} 
} //-- end while(1) -- 
} //-- end if(FILE_FLAG) -- 





// Open IMU shared memory 

if ((IMU_Shmid = OpenIMUShm()) == -1) 

{ 
printf ("Cannot Attach IMU Shared Memory\n") ; 
INIT_ERROR = 1; 





} 
if ((IMUFlag_Shmid = OpenIMUFlagShm()) == -1) 
{ 





printf ("Cannot Create IMU Flag Shared Memory\n"); 
INIT_ERROR = 1; 





} 
if (INIT_ERROR) exit(0); 
Reset IMUFlagShm (IMUFlag_Shmid) ; 








E 





// Open HMR shared memory to get psi value to initialize to 

if ((HMR_Shmid = OpenHMRShm()) == -1) 

{ 
printf ("Cannot Attach HMR Shared Memory\n"); 
INIT_ERROR = 1; 








sleep (2) ; // To ensure HMR has a value to initialize to 
ReadHMRShm (HMR_Shmid, &psi_out) ; 
CloseHMRShm (HMR_Shmid) ; 


printf ("Initial psi=%f degrees.\n", psi_out ); 
psi_out=psi_out*DegRad; 
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//-- Read IMU angle offsets from IMU.inp -- 
IMUalignfp = fopen("IMU.inp","r"); 
if(IMUalignfp) fscanf(IMUalignfp,"%lf 


$lf\n", &Align_theta, &Align_phi); 


fclose (IMUalignfp) ; 


//-- Read latitude from Nav.inp -- 
OpenInputFile() ; 
while (1) 
{ 
read_status = ReadFromInputFile(&FileString[0]); 
if (read_status > 0) 
{ 
sscanf (FileString, "%s",&FileCommand[0]); 
if (!stremp(FileCommand,"END")) break; 





} 
else if (!stremp (FileCommand, "SET_GPS_ORIGIN") ) 


{ 





sscanf (FileString,"%s S1f",é&FileCommand[0],&Lat0O) ; 
// Convert to dd.dddd 

LatD = (double) ((int) (Lat0/100.0)); 

Lat = LatD + (LatO - LatD*100.0)/60.0; 











} 
} 
CloseInputFile() ; 
s_Lat=sin(Lat*pi/180) ; 
c_Lat=cos (Lat *pi/180); 








//-- Open and flush the RS-422 port -- 
Port = open("/dev/ser4", O_RDONLY | O_NOCTTY); 
BytesRead = 2000; 
while (BytesRead == 2000) 
{ 





BytesRead = dev_read (Port, ReadBuf,2000,0,0,0,0,0); 
} 


// Initial load of message buffer 
BytesRead = dev_read (Port, ReadBuf,2000,44,0,0,0,0); 
for (i=0;1<44; i++)MsgBuf [i]=ReadBuf [i]; 


// SPK 011106: Create channel and connection for timer pulse 
timerChId = ChannelCreate (0) ; 
timerConId = ConnectAttach (ND_LOCAL_NODE, 0, timerChId, 





_NTO_SIDE_CHANNEL, 0); 





/* Attach to the Timer */ 
// SPK 011106: Initialize pulse event using Neutrino macro 
SIGEV_PULSE_INIT( é&event, timerConId, getprio(0), MP_PULSE_CODE, 0 




















// SPK 010306: Neutrino version returns timer ID in third parameter 
// old: LoopTimerId = timer_create (CLOCK_REALTIME, &event) ; 
timer_create (CLOCK_REALTIME, &event, &LoopTimerId) ; 
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if (LoopTimerId == -1) 

{ 
printf£( "Unable to Attach Timer." ); 
return; 
































/* 

* 1 nano-seconds before initial firing, 

* 1.0/Hz second repetitive timer afterwards. 

A/. 

opTimer.it_value.tv_sec = OL; 
oopTimer.it_value.tv_nsec = 1L; 
oopTimer.it_interval.tv_sec = OL; 
/* Convert Hz into NanoSecond Period */ 
oopTimer.it_interval.tv_nsec = (int) (1.0/((float) 


Hz) *pow(10.0,9.0)); 
timer_settime (LoopTimerlId, 0, &LoopTimer, NULL) ; 


[KOR KR KKK KK KK RK I IR I RI I KK RK OK / 


/*-— Main loop - once per shared memory write cycle 
(N_samples*1l10msec) --*/ 


[KOR KR KKK KK KK KK KR I I IR OR OR I OK / 
i_m=44; 
i_r=0; 
while (1) 
{ 
/* Wait for the Proxy */ 
// SPK 011106: No longer using old code (or mig4nto version) 
//old: Receive (LoopTimerProxy,0,0); 
timerRcvld = MsgReceive(timerChlid, &pulseMsg, sizeof (pulseMsg) , 
NULL) ; 





// SPK 011106: Can check if timerRcvId == 0 to verify a pulse was 
received and 
// can check if pulseMsg.code == MP_PULSE_CODE to verify it's 











from our timer 


[ORK RR KKK KK KK OK KK OK RK KK OK OR KK Ke / 





/*-- Read loop - once per IMU data message period wi 
[ROKK RR KKK KK KK RK KK KK OR KR OK KK / 


IMU_Error = 0; 





BytesRead = dev_read (Port, ReadBuf,2000,44,0,0,0,0); 
//printf£ ("bytes read = %d\n", BytesRead) ; 


LastMsgIdx = getLastMessage (ReadBuf, BytesRead) ; 


if (LastMsgIdx < 0 ) 





IMU_Error = 1; 
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// parse the messag 
p =*(short*) (ReadBuf+LastMsgldx+2); q 
=*(short*) (ReadBuf+LastMsgIdx+4) ; r =*(short*) (ReadBuft+tLastMsgIdx+6) ; 
ax =*(short*) (ReadBuf+LastMsgIdx+8); ay 
=* (short*) (ReadBuft+tLastMsgIdx+10); az 
=* (short*) (ReadBuft+tLastMsgIdx+12) ; 
statusl=* (unsigned short*) (ReadBuf+LastMsgIdx+14) ; 


status2=* (unsigned short 

















*) (ReadBuf+LastMsgIdx+16) ; 


























dAx=* (int*) (ReadBuf+LastMsgIdx+18) ; 
dAy=* (int*) (ReadBuf+tLastMsgIdx+22) ; 
dAz=* (int*) (ReadBuf+LastMsgIdx+26) ; 
dVx=* (int*) (ReadBuf+LastMsgIdx+30) ; 
dVy=* (int*) (ReadBuf+LastMsgIdx+34) ; 
dVz=* (int*) (ReadBuf+LastMsgIdx+38) ; 
msgCkSum=* (unsigned short*) (ReadBuf+LastMsgIdx+42) ; 
p_out (((double)p) * C_Rate); 
q_out = (((double)q) * C_Rate); 
r_out = (((double)r) * C_Rate); 
ax_out = (((double)ax) * C_Accel); 
ay_out = (((double)ay) * C_Accel); 
az_out = (((double)az) * C_Accel); 
// Note: IMU outputs negative acceleration values 
g = ((ax_out == 0.0) && (ay_out == 0.0) && (az_out == 0.0)) ? 


32. 


pow ( (pow (ax_out, 2) +pow(ay_out,2)+pow(az_out,2)),0.5); 


// Bound input to asin function to prevent NANs 


if 
{ 


theta_out 


} 


else 


{ 


theta_out 


(PITCH_LIMIT*DegRad) 
} 


( fabs (ax_out/g) 


* 


( 


’ 


(fabs (ax_out) ) 


1.0 ) 


asin (ax_out/g) ; 


* 


/ ) 


(ax_out) 


// Bound input to asin function to prevent NANs 


if ( fabs (ay_out/ (g*cos(theta_out))) < 1.0 ) 
{ 
phi_out = -asin(ay_out/ (g*cos(theta_out))); 
} 
else 
{ 
phi_out = —( (fabs(ay_out)) / (ay_out) ) * 
(ROLL_LIMIT*DegRad) ; 





} 





hi 


Remove earth rate, 


= sin(phi_out); 
sin(theta_out) ; 
= sin(psi_out); 
=(-c_phi*s_psi*c_Lat-s_phi*c_theta*s_Lat) *Omega; 


convert angular rates into heading rate 
= cos (phi_out); 
cos (theta_out) ; 
cos (psi_out); 


c_phi 
c_theta= 
c_psi 
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r_e=((s_phi*s_psi+c_phi*s_theta*c_psi) *c_Lat-— 
c_phi*c_theta*s_Lat) *Omega; 

q_out=q_out-q_e; 

r_out=r_out-r_e; 

psi_dot = s_phi*q_out/c_theta + c_phi*r_out/c_theta; 




















dPsi = psi_dot*dt; 
psi_out = psi_out+dPsi; 
} // iff (LastMsgIdx... else... 


ReadIMUFlagShm (IMUFlag_Shmid, &IMU_Kill); 
if (IMU_Kill) break; 





//--- SHARED MEMORY OUTPUT --- 











WriteIMUShm (IMU_Shmid, IMU_Id, phi_out,theta_out,psi_out,p_out, q_out, r_ou 
t,ax_out, 











ay_out,az_out,60.0,60.0,  IMU_Error) ; 
IMU_Id++; 
if (IMU_Id>65534) IMU_Id = 0; 


} // while(1) 
ConnectDetach( timerConId ); 


/* Get Rid of the Timer */ 
timer_delete (LoopTimerlId) ; 


if (FILE_FLAG) 


fclose (Outfp) ; 








close (Port); 

CloseIMUShm (IMU_Shmid) ; 
CloseIMUFlagShm (IMUFlag_Shmid) ; 
//CloseHMRShm(HMR_Shmid) ; 
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APPENDIX E: VEHICLE NAVIGATION FILTER CODE 


The following provides the C-code for the navigation filter. This code has been 
developed over many years of experience and was originally written by D. Marco and A. 


Healey for use in the ARIES vehicle. 





/* 
** SPK 010306: Modified for ONX Neutrino 6.3 
— comment out includes for obsolete files in Nav.h 
—- include migration library header file in Nav.h 
—- start the migration process manager (mig4nto_init) 
—- updated timer_create() to Neutrino version 
** SPK 010906: Replaced Creceive() with gqnx_proxy_detach () 
**x SPK 011106: Implemented Neutrino timer pulses instead of 
migration library cover functions 
—- no longer need migration process manager 

















include "Nav.h" 

// SPK 011106: Included for Neutrino timer pulses 
include <sys/neutrino.h> 

include <sys/netmgr.h> 





define TRUE 1 
define FALSE 0 








// SPK 011106: Added for Neutrino timer pulses 






























































define Nav_PULSE_CODE PULSE_CODE_MINAVAIL 
FILE *Outfp; 
FILE *NavErrorfp; 
FILE *Inputfp; 
FILE *AbortLogfp; 
char FileString[256]; /* This is the Entire Line of the File */ 











char FileCommand[256]; /* This is the File Command i.e. Leading 
Text */ 


double pi = 3.14159265358979; 
double RadDeg 57.2957 795% 
double DegRad = 0.01745329; 








/* Wait until RDI_AI1tEst is not equal to 0 - added by ajh 12/1/04 
Xf 


/* Altitude Filter Stuff */ 








int KALMAN_INIT = TRUE; 





/* added for RDI wait 12/01/04*/ 
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int WAIT = FALSE; 
int count = 0; 





doub]l Alt_est = 
doub] Alt_dot = 
doub]l Alt_ddot = 
double RDI_AltRawPrev = 





oo™Ne 








Oo Oo oO 


~e 








OOOO 
oOoOO°0O 
~ 


~“e 








FILE *TestFilefp; 


main () 


{ 


int read_status; 

double MaxDepth; 

double MinBatteryVoltage; 
double MaxLeakVoltage; 








nt i,j,k,kk,jj,1ip; 

nt INIT ERROR = FA 
nt ABORT_FLAG = FA 
nt KALMAN ABORT_FLAG = FA 
nt Nav_Kill = FA 














Heo Be BEB 








NHANNH 





/* Real Time Loop Stuff */ 
int Hz,t_count; 

double t,dt; 

pid_t LoopTimerProxy; 
timer_t LoopTimerld; 

struct itimerspec LoopTimer; 
struct sigevent event; 





// SPK 011106: Added for Neutrino timer 
int timerChId; 

int timerConlId; 

int timerRcvIid; 

struct _pulse pulseMsg; 


/* Absolute Date & Time Stuff */ 
struct timeb timebuf; 

time_t time_of_day; 

char buf [26]; 

struct tm tmbuf; 





char TacticalMessage[16]; 
/* date "+%smsdSysHsMsS" */ 


int Nav_Shmid; 
int NavFlag_Shmid; 
int NetFlag_Shmid; 
int RDI_Shmid; 
int MP_Shmid; 
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pulses 


int GPS_Shmid; 
int Analog_Shmid; 
int Bob_Shmid; 
int HMR _Shmid; 































































































double LatD, LongD; 

double KG_psi; 

int Nav_Id = 0; /* Navigation ID Process (1-65535) */ 

int Month; 

int Day; 

int Year; 

int Hour; /* 24 Hour GMT * / 

int Minute; 

double Second; /* Seconds and Fraction of Seconds */ 

double DepthRaw = 0.0; /* Raw Depth from Depth Cell */ 
/* (No Filtering) (m) */ 

double DepthEst = 0.0; /* Estimated Depth from Depth Filter (m) */ 

double DepthDot = 0.0; /* Estimated Depth Rate from Depth */ 
/* Filter (m/sec) x 

int Nav_Error = FALSE; 

int RDI_Id; /* RDI ID Process (1-65535) xf 

double RDI_Ug; /* RDI U Ground (m/sec) */ 

double RDI_Vg; /* RDI V Ground (m/sec) ef 

double RDI_Wg; /* RDI W Ground (m/sec) */ 

double RDI_Uf; /* RDI U Fluid (m/sec) a 

double RDI_Vf; /* RDI V Fluid (m/sec) x / 

double RDI_Wf; /* RDI W Fluid (m/sec) x / 

double RDI_AltRaw; /* Raw RDI Altitude (m) of 

double RDI_AltEst; /* Estimated Altitude from Alt Filter (m) x / 

double RDI_AltDot; /* Estimated Altitude Rate from Filter (m/sec) */ 

double RDI_Heading; /* RDI Heading (Degrees) x] 

double psi; /* RDI Heading (Radians) iA 

rite RDI_Error; 

int HMR_Id; /* HMR3000 Compass Process ID (1-65535) * / 

double HMR_HeadingRaw; /* HMR3000 Uncompensated Compass Heading 

(Degrees) */ 

double HMR_Heading; /* HMR3000 Compass Heading Angle (Degrees) */ 

double HMR_Pitch; /* HMR3000 Compass Pitch Angle (Degrees) ar 

double HMR_Roll; /* HMR3000 Compass Roll Angle (Degrees) xt 

int HMR_Error; 

/* Kalman Filter Stuff */ 

double RDI_A1ltRawComp; 





int AltZeroCount = 0; 
int SKIP_KALMAN = FALSE; 
double PrevRDI_AltRawComp = 0.0; 











// SPK 060206: Update variable definitions to reflect IMU process 
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// Note: 







































































variables 
int MP_Id; // IMU Process ID (1-65535) 
double MP_phi; // IMU Roll Angle (Rad) 
double MP_theta; // IMU Pitch Angle (Rad) 
double MP_psi; // IMU Heading Angle (Rad) 
double KG_q; // IMU Roll Rate (Rad/Sec) 
double MP_q; // IMU Pitch Rate (Rad/Sec) 
double MP_r; // IMU Yaw Rate (Rad/Sec) 
double MP_XAccel; // IMU X-axis Acceleration 
double MP_YAccel; // IMU Y-axis Acceleration 
double KG_r; // IMU Z-axis Acceleration 
double BatteryVoltageRaw; // Raw Battery Voltage Level 
double BatteryVoltage; // Filtered Battery Voltage Level (Volts 
int MP_Error; // IMU_Error 
int GPS_Id; /* GPS Process ID (1-65535) 
int GPS_Signal; /* Status of Signal 1 = Present, 
Present * / 
double Lat0; /* GPS Latitude Origin in ddmm.mmmmmm 
double Long0; /* GPS Longitude Origi 
double LatDeg0; /* GPS Latitude Origin in dd.ddddd 
double LongDeg0O; /* GPS Longitude Origin in ddd.ddddd 
double GPS_X; /* Distance in meters North/South from GPS 
Latitude */ 
/* Origin (Lat0) 
double GPS_Y; /* Distance in meters 
Longitude */ 
/* Origin (Long0) 
int Diff; /* Raw or Diff. 
int NSv; /* Number of SVs 
double ToC; /* Time of Computation 
double Lat; /* Latitude in ddmm.mmmmmm 
double LatDeg; /* Latitude in dd.ddddd 
double Long; /* Longitude in dddmm.mmmmmm 
double LongDeg; /* Longitude in ddd.ddddd 
double SbA; /* Sensor-Based Altitude 
double TtTc; /* True Track/True Course 
double SoG; /* Speed over Ground 
double Vv; /* Vertical Velocity 
double Pdop; /* Position dilution of position 
double Hdop; /* Horiz. dilution of position 
double Vdop; /* Vertical dilution of position 
double Tdop; /* Time dilution of position 
int GPS_Error; 





Analog_Id; 
BowLeakVol 





double 








MidLeakVol 





le 


doub 
doub 


le 
le 








tage; 


SternLeakVoltage; 


v_l1s; 
vV_rs; 




































































/* Analog Process ID (1-65535) 

/* Bow Section Leak Detector Voltage 
/* Level (Volts) 

/* Mid Section Leak Detector Voltage 
/* Level (Volts) 

/* Stern Section Leak Detector Voltage 
/* Level (Volts) 

/* Left Screw Speed (Rot/Sec) 

/* Right Screw Speed (Rot/Sec) 





9 


B 








still need to rename all process and shared memory 


(m/sec*2 
(m/sec*2 
(m/sec*2 





(Vol 








0 = Not 


n in dddmm.mmmmmm 





East/West from GPS 


Arf 
A 
aA 
s/s 
AY. 
*/- 
a 
*/, 
27/. 
*y; 
AY. 
Af 
A 
*/ 
ay 





*/ 


Ay. 


doub 
doub 


doub] 
doub]l 


int 


/* F 


feT 


doub]l 


doub]l 


CTO COO 0 


a. 
{@) 
c 
jon 





forse 
int 
int 
int 


int 


nt 
int 


- pe 


int 
int 
int 


// 8 





Qa 

1e) 

tay, 
CTO OF 




















le v_bvt; 

le v_svt; Vag 

e v_blt; /* 

e v_slt; {* 
Analog_Error; 


ilter Vars */ 


























/* Bow Vertical Thruster Speed (Rot/Sec) 
Stern Vertical Thruster Speed (Rot/Sec) 
Bow Lateral 17 
Stern Latera 




















[Thruster Speed (Rot/Sec) 





Origin (LatDeg0, LongDeg0) 
Y Position (meters) Relativ 


1 Thruster Speed (Rot/Sec) 


Estimates from the Filter) 
X Position (meters) Relativ 

















Origin (LatDeg0, LongDeg0) 


(Radians) 
(Rad/sec) 


Longitudinal Ground Speed (m/sec) 
Lateral Ground Speed (m/sec) 


Yaw Rate Bias (Rad/sec) 
Yaw angle Bias (Radians) 


,P_diag_out [9] 





,Pphi_t[(9][9],phiPphi_t[9] [9]; 


,Lres[9] [2]; 


hese are States (Outputs, 
le NavFil_xX; /* Global 
/* a GPS 
le NavFil_yY; /* Global 
/* a GPS 
le NavFil_psi; /* Yaw Angle 
le NavFil_r; /* Yaw Rate 
le NavFil_Ug; /e 
le NavFil_Vg; /* 
le NavFil_Bias_r; /* 
le NavFil_Bias_psi; /* 
le y[1][8],ym[8] 
le I18[9] [9]; 
le x[9]; 
le x_barl1[9] 
le res[8][2],ym_prev[8],yhat[8]; 
le psi0; 
le spsi,cpsi; 
le Adt[9] [9]; 
le C[8] [9]; 
le 
7M[9][9],LC[9][9],ImLC[9] [9] 
le phi[9][9],phi_t[9] [9] 
le C_t[9][8],MC_t[9][8],CMC_t[8] [8]; 
le CMCR[8] [8],CMCRI[8][8],L[9] [8] 
le Q[9] [9]; 
le R[8] [8]; 
le q[9],nu[8],p[9]; 
rocess Vars */ 


LastRDI_Id 
RDI_IdDupCnt 
MaxRDI_IdDupCnt 





LastGPS_Id 
GPS_IdDupCnt 
MaxGPS_IdDupCnt 


LastMP_Id 
MP_IdDupCnt 
MaxMP_IdDupCnt 
PK 071806 
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to 


to 


AY]: 
fe 
&f 
Af 
aif 
a7, 


*/ 
ay 
ay 
a. 


a7, 
A 
a7 
A 
ae 


,P_1_8_out,P_2_ 8 out; 





int LastMP_Error = 0; 








int MP_ErrorCnt = 0; 
int MaxMP_ErrorCnt = 10; 
int LastAnalog_Id = 0; 
int Analog_IdDupCnt = 0; 


int MaxAnalog_IdDupCnt = 10; 





int LastHMR_Id = 0; 
int HMR_IdDupCnt = 0 
int MaxHMR_IdDupCnt = 10; 


double Coefl; 


Coefl = 3443.9* (1852.47) * (pi/180.0); 
/*Coefl = 111318.8938906694; */ 


for (1=1; 1<=8; ++i) 

{ 
for (j=1; 3<=8;++)9) 
{ 

j] = 


tow ol 
oo 20-0 
MON NEN 


2 - OO: © © 
~ 


~“e 


t[il 
[i] [3 
[i] (3 
[i] (3 
8filfj] = 


for (1=1; 1<=8; ++i) 

{ 
I8[i] [i] = 1.0; 
x_barl[i] = 0.0; 


for (1=1; 1<=7; ++i) 
{ 
for (j=1; J<=7; +19) 


for (1=1;1<=7; ++i) 
{ 
for (j=1; 3<=8;++)) 


/* Measurement Vector: 
y = [Ug,Vg,psi,r, LatDeg, LongDeg] ; 
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af 


/* New Measurement Vector: 
y = [Ug,Vg,psi,r,LatDeg, LongDeg, KG_psi]; 
KG_psi = Integrted KG_r; 





ei 

Hz = 8; 

t = 0.0; 
t_count = 0; 


dt = 1.0/((double) Hz); 








NavErrorfp = fopen("NavError.d","w"),; 
AbortLogfp = fopen("Abort.Log","w") ; 

OpenNavDataFile() ; 

OpenInputFile() ; 

if ((Nav_Shmid = OpenNavShm()) == -1) 


{ 
printf ("Cannot Attach Nav Shared Memory\n"); 
INIT_ERROR = TRUE; 








if ((NavFlag_Shmid = OpenNavFlagShm()) == -1) 
{ 
printf ("Cannot Create NavFlag Shared Memory\n"); 
INIT_ERROR = TRUE; 








if ((NetFlag_Shmid = OpenNetFlagShm()) == -1) 
{ 
printf ("Cannot Create NetFlag Shared Memory\n"); 
INIT_ERROR = TRUE; 











if ((RDI_Shmid = OpenRDIShm()) == -1) 


printf ("Cannot Attach RDI Shared Memory\n"); 
INIT_ERROR = TRUE; 








if ((MP_Shmid = OpenMotPakShm()) == -1) 


printf ("Cannot Attach MotPak Shared Memory\n"); 
INIT_ERROR = TRUE; 














if((GPS_Shmid = OpenGPSShm()) == -1) 
{ 
printf ("Cannot Attach GPS Shared Memory\n") ; 
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INIT_} 





Gr 
E 


RROR = TRUE; 





if ((Analog_Shmid = OpenAnalogShm()) == -1) 


{ 


printf ("Cannot Attach Analog Shared Memory\n"); 


INIT_} 


ry 
Ei 





if ((Bob_S 


{ 


printf 
INIT_1 


if ( (HMR_ 


{ 


Ge 
bE 





S) 


printf 





Bh 

















E 





RROR = TRUE; 





hmid = OpenBobShm()) == -1) 


("Cannot Attach Bob Shared Memory\n"); 
RROR = TRUE; 





hmid = OpenHMRShm()) == -1) 


("Cannot Attach HMR Shared Memory\n"); 
RROR = TRUE; 








RROR) exit (0); 


ResetNavFlagShm (NavFlag_Shmid) ; 


WriteTacticalMessage (NetFlag_Shmid,"GO") ; 


/* Get Initialization Values */ 
while (TRUE) 


{ 


read_s 








tatus = ReadFromInputFile (é&FileString[0]); 


if (read_status > 0) 


{ 


sscanf (FileString, "%s",&FileCommand[0]); 


/* Break if End */ 
if (!stroemp (FileCommand, "END") ) 


{ 


} 








break; 


else if (!stremp (FileCommand, "SET_MAX DEPTH") ) 


{ 


} 








sscanf (FileString,"%s S1f",&FileCommand[0],&MaxDepth) ; 


else if (!stremp (FileCommand, "SET_GPS_ORIGIN") ) 


{ 





sscanf(FileString,"%s S1lf Sl1f",&FileCommand[0], 
&Lat0O, &Long0O) ; 
/* Convert to dd.dddd */ 


LatD = (double) ((int) (Lat0/100.0)) 














LongD = (double) ((int) (Long0/100.0)); 
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LatO - LatD*100.0)/60.0; 
LongO - LongD*100.0)/60.0; 


— 


LatDeg0 LatD + 
LongDegO = LongD + 

















— 


} 
else if (!stremp (FileCommand, "SET_MIN_BATTERY_VOLTAGE") ) 


{ 














sscanf (FileString,"%s 
Sli", &FileCommand[0], &MinBatteryVoltage) ; 
} 
else if (!stremp (FileCommand, "SET_MAX LEAK VOLTAGE") ) 


{ 














sscanf (FileString,"%s 
Sli", &FileCommand[0], &MaxLeakVoltage) ; 
} 
else 


{ 
printf ("FileCommand Not Recognized\n") ; 


CloseInputFile() ; 





/* Read Initial Measurement Vector and Wait until RDI_AltRaw is not 
equal to 0 - ajh 12/1/04 */ 

while (WAIT) 

{ 


ReadRDIShm(RDI_Shmid, &RDI_Id, &RDI_Ug, &RDI_Vg, &RDI_Wg, 
&RDI_Uf, &RDI_Vf, &RDI_Wf, 
&RDI_AltRaw, &RDI_Heading, 
&RDI_Error); 





sleep (1); 
if (RDI_AltRaw > 0.0) 
{ 
count = counttl1; 
} 
if (count == 3) 


{ 
WAIT=FALSE; 





LastRDI_Id = RDI_Id; 


/* Read Initial Measurement Vector */ 





ReadHMRShm (HMR_Shmid, &HMR_Id, £HMR_HeadingRaw, &HMR_Heading, &HMR_Pitch, 
&HMR_ Roll, 
&HMR_Error); 





LastHMR_Id = HMR_Id; 


ReadMotPakShm (MP_Shmid, &MP_Id, &MP_phi, &MP_theta, &MP_psi, 
&KG_q, &MP_q, &MP_r, 
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&MP_XAccel, &MP_YAccel, &KG_r, &BatteryVoltageRaw, 


&BatteryVoltage, 
&MP_Error); 





// SPK 09/01/05 
psi = MP_psi; 


LastMP_Id = MP_Id; 
// SPK 071806 
LastMP_Error = MP_Error; 











ReadGPSShm(GPS_Shmid, &GPS_Id, &GPS_Signal, &Diff, &NSv, &ToC, &éLat, &LatDeg, 


&V. 


&Long, &LongDeg, &SbA, &TtTc, &So0G, &Vv, 
&Pdop, &Hdop, &Vdop, &Tdop, &GPS_Error); 





LastGPS_Id = GPS_Id; 





ReadAnalogShm (Analog_Shmid, &Analog_Id, &DepthRaw, &DepthEst, &DepthDot, 
&BowLeakVoltage, &MidLeakVoltage, &SternLeakVoltage, 





ls,&v_rs,&v_bvt, év_svt, &v_blt, &v_slt, &Analog_Error); 





LastAnalog_Id = Analog_Id; 


if (!GPS_Signal) 

{ 
/* We Don't have a GPS Signal, Zero GPS_X and GPS_Y */ 
GPS_X = 0.0; 
GPS_Y 0.0; 


} 
else 
{ 
GPS_X = Coefl*(LatDeg - LatDeg0O); 
GPS_Y = Coefl1*(LongDeg - LongDeg0) *cos (DegRad*LatDeg) ; 





/* Assign Initial Measurement Vector */ 
ym[1] = RDI_Ug; 





// SPK 09/01/05 
//ym[2] = RDI_Vg-1.0*KG_r; 
ym[2] = RDI_Vg-1.0*MP_r; 


// SPK 021606: Add delay to ensure we read the latest value from IMU 
sleep (1); 
ym[3] = psi; 





// SPK 09/01/05 
//ym[4] = KG_r; /* Was MP_r */ 
ym[4] = MP_r; 
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K 
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oa 
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= GPS_xX; 
ym[6] = GPS_Y; 


// SPK 09/01/05 
//ym[7] = KG_psi; /* This is a Virtual Heading Ref. */ 
ym[7] = psi; 





/* State Vector: 
x = [NavFil_X NavFil_Y NavFil_psi NavFil_r NavFil_Ug NavFil_Vg 
NavFil_Bias_r NavFil_Bias_psi]; */ 














x[1l] = ym[5]; /* Initial GPS_X */ 

x[2] = ym[6]; /* Initial GPS_Y */ 

x[3] = ym[3]; 

x[4] = ym[4]; 

x[5] = ym[1]; 

x[6] = ym[2]; 

x[7] = 0.0; /* Bias = 0 at Start */ 
x[8] = 0.0; /* Bias = 0 at Start */ 











/* Init the State Estimate Vector */ 









































NavFil_X = x[1]; 

NavFil_Y = x[2]; 

NavFil_psi = x[3]; 

NavFil_r = x[4]; 

NavFil_Ug = x[5]; 

NavFil_Vg = x[6]; 

NavFil_Bias_r = x[7]; 

NavFil_Bias_psi = x[8]; 

spsi = sin(NavFil_psi); 

cpsi = cos (NavFil_psi); 

Adt[1] [3] = (-NavFil_Ug*spsi-NavFil_Vg*cpsi) *dt; 

Adt[1] [5] = cpsi*dt; 

Adt[1] [6] = -spsi*dt; 

Adt [2] [3] = (NavFil_Ug*cpsi-NavFil_Vg*spsi) *dt; 

Adt [2] [5] = spsi*dt; 

Adt [2] [6] = cpsi*dt; 

Adt [3] [4] = 1.0*dt; 

C[{1] [5] = 0; 

C[2] [6] = 1.0; 

C[3] [3] = 0; 

C[3] [8] = 1.0; /* 120303 Mod to eliminate Bias learning for Compass 
Tests. returned to 1.0 111904 */ 

C[4] [4 1 0y 

C[4] [7] = 0.0; // SPK 061406: Force NavFil_Bias_r to 0.0 

C[5] [1] = 1.0; 

Cl6é] [2] = 1.0; 

C[7] [3] = 0.0; // measurement 7 is not necessary when using the IMU 
psi 
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0.0; // measurement 7 is not necessary when using the IMU 


/* q[3] 


ll 
1Q 
ws 
ll 
jo) 
(exo) 
KR 
~ 
1Q 
ol 
ll 
1Q 
Oo 
ll 
jo) 
KR 
~ 
1Q 
~ 
ll 
jo) 
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/* Orig Vals returned to original 111904 - ajh 








q{1] = 0.0; 

q{2] = 0.0; 

g[3] = 0.001; 

q[4] = 0.1; 

q{[5] = 0.01; 

q[6] = 0.01; 

g[7] = 0.0000001; 
q{8] = 0.000001; */ 


/* New Vals */ 

















q{l] = 0.0; /* variance on LatDeg */ 

q{2] = 0.0; /* variance on LongDeg as 

q{[3] = 0.001; /* variance on psi, (Radians)*2 returned 111904 

AY. 

q{4] = 0.1; /* variance on r, rad/sec)%*2 */ 

q{5] = 0.01; /* variance on Ug, (m/sec) *2 x 

q{6] = 0.01; /* variance on Vg, (m/sec) *2 */ 

q{7] = 0.0000001; /* variance on NavFil_Bias_r, (Rad/sec) */ 

q[8] = 0.0; /* variance on NavFil_Bias_psi (Radians) */ 





/* Create Diagonal Q Matrix */ 
for (1=1;1<=8; ++i) 


Li walt) SS O:0L 
fi) DaAl2) = 0013 
// nu[3] = 0.1; 

// nal4] = 0.001; 
// nal5] = 1.0; 

// nul6] = 1.0; */ 
/* New Vals */ 





// SPK 081606: Default values 
nu[1l] = 0.001; 

nu[2] = 0.0001; 

nu[3] = 0.001; 

nu[4] = 0.001; 

nu[5] = 1.0; 
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nu[6] = 
nu[7] = 


RoR 


~“e 


// SPK 081606: Set elements of R Matrix based on NSv (as verified by 
SRV 08/2006) 
if (NSv <= 3) 


else if (NSv >= 5) 


/* Create Diagonal R Matrix */ 
for (1=1;1<=7; ++i) 
{ 

R[i] fi] = nulil; 

/* REilfil = nulil; */ 


aS 





CDOOOCOCOC 0 
DOO OCOC 0 
~ 








OotOr ONO. 462 $0" "0 
OINHDHOBWNE 
ll 


/* Create Diagonal P Matrix */ 
for (1=1;1<=8; ++i) 


// SPK 060106: Write initialized values to first line of NavD file 
time_of_day = time (NULL); 


gmtime_r(&time_of_day, &tmbuf) ; 
ftime (&timebuf) ; 

Month = tmbuf.tm_mon+l; 

Day = tmbuf.tm_mday; 

Year = tmbuf.tm_year+1900; 
Hour = tmbuf.tm_hour; 

Minute = tmbuf.tm_min; 





Second ((double) (tmbuf.tm_secttimebuf.millitm/1000.0)); 
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fprintf (Outfp, "% 
Sd Sf % 
S£ Sd SF 


ol? 


Sd sd %d 
Sf Sf SF 
SE S£ %£\n", 
Nav_Iid, 
Hour, 
Minute, 
Second, 
MP_Id, 
KG_q, 
MP_q, 
MP_r, 
RDI_Id, 
RDI_Ug, 
RDI_Vg, 
R 
R 
R 
R 
0 


ol? 
rh OQ. fh 


ol? 





DI_Wg, 
DI_Uf, 
DI_V£, 
DI_WE, 





initialized yet 








initialized yet 





initialized yet 




















.0,//RDI_AltRawComp, 
RDI_AltRaw, //RDI_AltEst, 


0.0,//RDI_AltDot, 


ol? 
ole 
ol? 
ol? 
ole 
ol? 
ole 


ae ol? 
rh Fh QO. 
al ol 
Hh Fh QO. 
ae ol? 
Fy FH Fh 
al ol 
Hh Fh QO. 
ol 0 
Fr FH Fh 
al ol 
Fr Fh Fh 
oe fo\\e) 
Fr FH Fh 


//SPK: use 





//SPK: 


//SPK: use 


RDI_Heading, 
GPS_Id, 
GPS_Signal, 
Dirty 

NSv, 

ToC, 

LatDeg, 
LongDeg, 
TETC; 
DepthRaw, 
DepthEst, 
DepthDot, 
MP_phi, 
MP_theta, 
v_is, 

v_rs, 
NavFil_x, 
NavFil_Y, 
NavFil_psi, 
NavFil_r, 
NavFil_Ug, 
NavFil_Vg, 
NavFil_Bias_psi, 
NavFil_Bias_r, 


Analog_Id, 


BatteryVol 
BatteryVol 
GPS_X, 
GPS_Y, 
HMR_Id, 


ltageRaw, 
ltage, 
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sd sf Sf Sf Sf Sf sf Sf Sf 
Sf sf sf Sf Sf Sf sf Sf Sf 
Sf Sf Sf Sf Sf Sf Sf Sf SF 


0.0 because AltRawComp not 


use AltRaw because AltEst not 


0.0 because AltDot not 


oe ol? 


ole 


Fh Fh Fh 













































































HMR_HeadingRaw, 
HMR_Heading, 
HMR Pitch, 
HMR_Roll, 
KG_r, 
psi, 
Pdop, 
Hdop, 
Vdop, 
Tdop, 
MP_XAccel, 
MP_YAccel, 
P[1][1], // SPK: initial diagonal elements of P matrix 
P[2][2], // SPK: initial diagonal elements of P matrix 
P[3][3], // SPK: initial diagonal elements of P matrix 
P[4][4], // SPK: initial diagonal elements of P matrix 
P[5][5], // SPK: initial diagonal elements of P matrix 
P[6][6], // SPK: initial diagonal elements of P matrix 
P[7][7], // SPK: initial diagonal elements of P matrix 
P[8][8], // SPK: initial diagonal elements of P matrix 
P[1][8], // SPK: initial 1,8 element of P matrix 
P[2][8]);// SPK: initial 2,8 element of P matrix 

// SPK 060106: End write of initialized values to first line of NavD 











file 


// SPK 011106: Create channel and connection for timer pulse 

timerChId = ChannelCreate (0) ; 

timerConId = ConnectAttach (ND_LOCAL_NODE, 0, timerChId, 
_NTO_SIDE_CHANNEL, 0); 














/* Get a Proxy for the Timer to Kick */ 
S 


// SPK 011106: No longer using the old code (or mig4nto version) 
/* 
LoopTimerProxy = qnx_proxy_attach( 0, 0, 0, -1 ); 
if (LoopTimerProxy == -1) 
{ 
printf( "Unable to Attach Proxy." ); 
return; 
} 
ay; 


/* Attach to the Timer */ 

// SPK 011106: Initialize pulse event using Neutrino macro 

// old: event.sigev_signo = -LoopTimerProxy; 

SIGEV_PULSE_INIT( sevent, timerConId, getprio(0), Nav_PULSE_CODE, 0 














// SPK 010306: Neutrino version returns timer ID in third paramete 
// old: LoopTimerId = timer_create (CLOCK_REALTIME, é&event) ; 


























timer_create (CLOCK_REALTIME, &event, &LoopTimerId) ; 
if (LoopTimerId == -1) 
{ 

printf( "Unable to Attach Timer." ); 





return; 
































/* 
* 1 nano-seconds before initial firing, 
* 1.0/Hz second repetitive timer afterwards. 
AY 
opTimer.it_value.tv_sec = OL; 
opTimer.it_value.tv_nsec = 1L; 
oopTimer.it_interval.tv_sec = OL; 
/* Convert Hz into NanoSecond Period */ 
oopTimer.it_interval.tv_nsec = (int) (1.0/((float) 


Hz) *pow(10.0,9.0)); 
timer_settime (LoopTimerId, 0, &LoopTimer, NULL) ; 


while (TRUI 
{ 


1 | 
~~ 





/* Wait for the Proxy */ 

// SPK 011106: No longer using old code (or mig4nto version) 
//old: Receive (LoopTimerProxy,0,0); 

timerRcvld = MsgReceive(timerChIid, &pulseMsg, sizeof (pulseMsg) , 








NULL) ; 

// SPK 011106: Can check if timerRcvId == 0 to verify a pulse was 
received and 

// can check if pulseMsg.code == Nav_PULSE_CODE to verify it's 











from our timer 
/* Do Work */ 
expAdt 88 (SAdt, &phi, 8) ; 


spsi = sin(* (x4 
cpsi = cos (* (x1 


ies ust 
WW 
~ 





~“e 








x_barl 
x_barl 
x_barl 
x_barl 


* ( 5)*cpsi - *(x+6)*spsi) *dt; 
*( 

*( 

* ( 

*(x_barl 

#( 

* 

* ( 


(* (x 
(*(x+5)*spsi + *(x+6) *cpsi) *dt; 
* (xt 



































~“e 








x_barl 
x_barl 
x_barl 


~“e 








+ + + F F F HF 
Ne 


x me xe Me KX KM KM KX 


~“e 











AANA PWNE 
~‘e 





OANA PWNE 





~“e 


transpose (&phi, &phi_t, 8,8); 
AdotB(&P, &phi_t, &Pphi_t,8,8,8); 
AdotB(&phi, &Pphi_t, &éphiPphi_t,8, 8,8); 
ApmB (&phiPphi_t, &Q, &M,8,8,'+'); 




















*(yhatt+l) = *(x_barl+t5); 
*(yhat+2) = *(x_barl1+6); 
*(yhat+3) = *(x_bar1+3) * (x_barl1+8); 
*(yhat+4) = *(x_barl1+4) *(x_barl+7); 
*(yhat+5) = *(x_barl+l1); 





(yhat+7) = *(x_barl1+3) + *(x_barl1+8); 








ReadRDIShm(RDI_Shmid, &RDI_Id, &RDI_Ug, &RDI_Vg, &RDI_Wg, 
&RDI_Uf, &RDI_Vf, &RDI_Wf, 
&RDI_AltRaw, &RDI_Heading, 
&RDI_Error) ; 





/* Check for HungUp Process */ 
if (RDI_Id == LastRDI_Id) 
{ 
++RDI_IdDupCnt; 
} 
else 
{ 
RDI_IdDupCnt = 0; 
} 
if (RDI_IdDupCnt >= MaxRDI_IdDupCnt) 
{ 
fprintf (AbortLogfp, "MaxRDI_IdDupCount = %d @ RDI_Id = %d\n", 
RDI_TdDupCnt,RDI_Id); 
fflush (AbortLogfp) ; 
WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 
ABORT_FLAG = TRUE; 





} 
LastRDI_Id = RDI_Id; 


/* psi = RDI_Heading*DegRad; */ 


ReadHMRShm (HMR_Shmid, &HMR_Id, &HMR_HeadingRaw, &HMR_Heading, 
&HMR_Pitch, &HMR_Rol1l, &HMR_Error); 





/* Check for HungUp Process */ 
if (HMR_Id == LastHMR_Id) 
{ 





++HMR_IdDupCnt ; 
} 
else 
{ 
HMR_IdDupCnt = 0; 
} 
if (HMR_IdDupCnt >= MaxHMR_IdDupCnt) 
{ 
fprintf (AbortLogfp, "MaxHMR_IdDupCount = %d @ HMR_Id = %d\n", 
HMR_IdDupCnt, HMR_Id); 
fflush (AbortLogfp) ; 
WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 
ABORT_FLAG = TRUE; 
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LastHMR_Id = HMR_Id; 


/*xk*kk*kx*k* This is the New psi for the EKF ******x*/ 





// SPK 09/01/05 
//psi = HMR_Heading*DegRad; 


/* psi = RDI_Heading*DegRad; Lys 
// SPK 09/01/05 


/* Integrate KG_r to obtain KG_psi */ 
//KG_psi = KG_psi + dt*KG_r; 


/* BAdded to eliminate KG problems 10 Sep 02 */ 
/* KG_psi = psi; */ 
ReadMotPakShm (MP_Shmid, &MP_Id, &MP_phi, &MP_theta, &MP_psi, 


&KG_q, &MP_q, &MP_r, 
&MP_XAccel, &MP_YAccel, &KG_r, 





&BatteryVoltageRaw, &éBatteryVoltage, &MP_Error); 





// SPK 09/01/05 
psi = MP_psi; 


/* Bias KG_r */ 


/* Perform bias correction in MotPakf.c instead -- SPK 3/22/04 
KG_r = KG_r - 1.219e-4; 
* 


/* Check for HungUp Process */ 
if (MP_Id == LastMP_Id) 
{ 
++MP_TdDupCnt; 
} 
else 
{ 
MP_IdDupCnt = 0; 
} 
if (MP_IdDupCnt >= MaxMP_IdDupCnt) 
{ 
fprintf (AbortLogfp, "MaxMP_IdDupCount = %d @ MP_Id = %d\n", 
MP_IdDupCnt,MP_Id); 
fflush (AbortLogfp) ; 
WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 
ABORT_FLAG = TRUE; 





} 
LastMP_Id = MP_Id; 
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ReadGPSShm(GPS_Shmid, &GPS_Id, &GPS_Signal, &Diff, &NSv, &ToC, &éLat, &LatDeg, 
&Long, &LongDeg, &SbA, &TtTc, &S0G, &Vv, 
&Pdop, &Hdop, &Vdop, &Tdop, &GPS_Error) ; 





// SPK 081606: Set elements of R Matrix based on NSv (as verified by 
SRV 08/2006) 
if (NSv <= 3) 


a Oi 
20% 


else if (NSv == 4) 


else if (NSv >= 5) 
{ 


/* Update Diagonal R Matrix */ 
for (1=1; 1<=7; ++i) 
{ 

R[i] [i] = nulil; 


/* Check for HungUp Process */ 
if (GPS_Id == LastGPS_Id) 
{ 
++GPS_TIdDupCnt; 
} 
else 
{ 
GPS_IdDupCnt = 0; 
} 
1f(GPS_IdDupCnt >= MaxGPS_IdDupCnt) 
{ 
fprintf (AbortLogfp, "MaxGPS_IdDupCount = %d @ GPS_Id = %d\n", 
GPS_IdDupCnt,GPS_Id); 
fflush (AbortLogfp) ; 
WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 
ABORT_FLAG = TRUE; 





} 
LastGPS_Id = GPS_Id; 


if (!GPS_Signal) 
{ 
/* We Don't 
GPS_X = 0.0; 
GPS_Y 0%.0:7 


have a GPS Signal, Zero GPS_X and GPS_Y */ 
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GPS_X = Coefl*(LatDeg - LatDeg0); 
= Coefl* (LongDeg - LongDeg0Q) *cos (DegRad*LatDeg) ; 





a 
‘U 
36 
K 
| 





ReadAnalogShm (Analog_Shmid, &Analog_Id, &DepthRaw, &DepthEst, &DepthDot, 
&BowLeakVoltage, &MidLeakVoltage, &SternLeakVoltage, 





&év_ls,&v_rs,&v_bvt, év_svt, &v_blt, &v_slt, &Analog_Error) ; 








/* Check for HungUp Process */ 
if (Analog_Id == LastAnalog_Id) 
{ 

++Analog_IdDupCnt; 
} 
else 
{ 

Analog_IdDupCnt = 0; 
} 
if (Analog_IdDupCnt >= MaxAnalog_IdDupCnt) 
{ 





fprintf (AbortLogfp, "MaxAnalog_IdDupCount = %d @ Analog_Id = 
sd\n", 





Analog_IdDupCnt,Analog_Id); 
fflush (AbortLogfp) ; 
WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 
ABORT_FLAG = TRUE; 





} 
LastAnalog_Id 





Analog_Id; 


/* Assign the Measurements */ 
*(ym+1) = RDI_Ug; 


// SPK 09/01/05 


//*(ym+2) = RDI_Vg-1.0*KG_r; /* This Takes into Account the RDI 
Offset */ 

*(ym+2) = RDI_Vg-1.0*MP_r; 

*(ym+3) = psi; 





// SPK 09/01/05 
//*(ym+4) = KG_r; /* Was MP_r */ 
*(ymt+4) = MP_r; 


*(ym+5) = GPS_X; 
*(ym+6) = GPS_Y; 





// SPK 09/01/05 
//*(ym+7) = KG_psi; /* This is a Virtual Heading Ref. */ 
*(ym+7) = psi; 





for (j=1; 3<=7;++)) 
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{ 
res[j] [1] = *(ym+j) - *(yhat+)j); 
} 


/* To Kill Spikes from RDI */ 


/* Ug & Vg */ 
for (k=1;k<=2;++k) 
{ 
if( (*(ymt+k) == *(ym_prevtk)) || (fabs (* (ymtk)) > 5.0 ) 
|| (*(ymt+tk) == 0.0) ) 
{ 
for (j=1; 3<=8;++)) 


/* Check Psi & r */ 
for (k=3;k<=4; ++k) 
if (fabs( *(ymtk) - *(ym_prevtk) ) < 0.000001) 
for (j=1; 3<=8;++)) 
| C[Ik] [3] = 0.0; 


} 


/* Check X & Y */ 
for (k=5;k<=6;++k) 
if( fabs( *(ymt+k) - *(ym_prevtk) ) < 0.000001 ) 
for (j=1; 7<=8;++)) 
| CIk] [3] = 0.0; 


} 


if(!GPS_Signal) 
{ 
/* We Don't a GPS Signal, Zero the Rows */ 
for (k=5;k<=6;++k) 
{ 
for (j=1; 3<=8;++)9) 
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transpose (&C, &C_t,7,8); 


AdotB (&M, &C_t, &MC_t,8,8,7); 
AdotB(&C, &MC_t, &CMC_t,7,8,7); 
ApmB (&CMC_t, &R, &CMCR,7,7,'+'); 
inv (&CMCR, &CMCRI,7); 

AdotB (&MC_t, &CMCRI, &L,8,7,7); 


AdotB(&L,&C, &LC,8,7,8); 
ApmB (&18,&LC, &ImLC, 8,8, '-'); 
AdotB (&ImLC, &M, &P,8,8,8); 


// SPK 11/21/05: store diagonal elements of P matrix 
for (ip=1; ip<=8; ipt+t+) 
{ 





P_diag_out[ip] = P[ip] [ip]; 
} 


// SPK 04/17/06: store elements 1,8 and 2,8 of P matrix 
P_1_8 out = P[1][8]; 
P_2_8 out = P[2][8]; 


AdotB(&L, éres, &Lres,8,7,1); 
for (j=1; 3<=8;++)9) 
{ 
*(xtj) = *(x_barlt+j) + Lres[j] [1]; 
} 


// SPK 052406: Force NavFil_Bias_r to 0.0 

// SPK 061406: Undo this change and do it by zeroing the C-Matrix 
elements 

//*(x+7) = 0.0; 











spsi = sin(* (x+3))j; 

cpsi = cos (*(x+3)); 

Adt [1] [3 ( —(* (x+5) *spsi)-—(* (x+6))*cpsi )*dt; 
Adt[1] [5] = cpsi*dt; 

Adt[1][6] = -spsi*dt; 

Adt [2] [3] = ( *(x+5)*cpsi —- (*(x+6))*spsi) *dt; 

Adt [2] [5] = spsi*dt; 

Adt [2] [6] = cpsi*dt; 

Adt [3] [4] = 1.0*dt; 

Orly Fed. -S"1020% 

C[2] [6] = 1.0; 

C[3] [3] = 1.0; 

C[3]{8] = 1.0; /* Turn on Bias learning 090105 ajh */ 
C[4] [4] = 1.0; 

C[4] [7] = 0.0; // SPK 061406: Force NavFil_Bias_r to 0.0 
Cf{5] [1] = 0; 
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// SPK 09/01/05 














//C[7] [3] = 1.0; 

CLT) [3] -=-0:.0; 

C{[7] [8] = 0.0; /* Mod to turn off Bias learning 050405 ajh */ 
NavFil_X (XL) 3S 

NavFil_Y = *(xt+2); 

NavFil_psi = *(xt+3); 

NavFil_r = *(xt4); 

NavFil_Ug = *(xt+5); 

NavFil_Vg = *(xt+6); 

NavFil_Bias_r Ss PE) 

NavFil_Bias_psi = *(x+8)j; 

/* Compensate for Pitch & Roll */ 

PrevRDI_AltRawComp = RDI_AltRawComp; 

RDI_AltRawComp = RDI_AltRaw*cos (MP_phi) *cos (MP_theta) ; 





/* Check for Sequential Zeros */ 
if (RDI_AltRawComp == 0.0) 
{ 
++AltZeroCount; 
if (AltZeroCount<24) 
i 
SKIP_KALMAN = TRU 
RDI_A1tDot = 0.0; 





GJ 


} 

else 

{ 
/* Too many Segential Holidays */ 
/* KALMAN ABORT_FLAG = TRUE; */ 
SKIP_KALMAN = TRUE; 














} 

else 

{ 
AltZeroCount = 0; 
if (PrevRDI_AltRawComp == 0.0) 
{ 





SKIP_KALMAN = FALSE; 
KALMAN_INIT TRUE; 








/* AltEst and Alt_dot are Filtered Outputs from the Kalman Filter 
*/ 

if (! SKIP_KALMAN) 

{ 


AltitudeKalman (RDI_AltRawComp, &RDI_Al1tEst, &RDI_AltDot, &KALMAN_ABORT_FLA 
G) ; 





} 
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sf\n",MaxDepth) ; 


Compartment \n") ; 
fprintf (AbortLogfp, "BowLeakVoltage 





if (KALMAN_ABORT_FLAG == TRU 
{ 


if (ABORT_FLAG 
{ 


FALSE 





7 
~~ 





WriteTacticalMessage (NetF] 


fprintf (AbortLog 

"Abort 
fprintf (AbortLog 
fprintf (AbortLog 
fflush (AbortLogfp) ; 
ABORT_FLAG TRUE; 


fp, 











if((DepthRaw > MaxDept 
{ 


if (ABORT_FLAG 
{ 


FALSE 








fp, "RDI_Alt 
fp, "RDI_Alt 


GJ 
~~ 








Est 








lag_Shmid, "ABORT") ; 


Due to Unstable Altitude Kalman Filter\n"); 
Raw = %f\n",RDI_Alt 
%£\n",RDI_Alt 


Raw); 
Est); 











(DepthEst > MaxDepth) ) 


WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 


fprintf (AbortLog 


fprintf (AbortLog 
fprintf (AbortLog 
fflush (AbortLogfp) ; 
ABORT_FLAG TRUE; 











if (BatteryVoltage < Mi 
{ 


if (ABORT_FLAG 
{ 


FALSE) 








fp, "DepthRaw 
fp, "DepthEst 


fp,"Abort Due to 





nBatteryVoltage) 





Exceeding MaxDepth 


sf\n",DepthRaw) ; 
sf\n",DepthEst) ; 





WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 


fprintf (AbortLogfp, 


"Abort Since Below Minimum Battery Voltage 
MinBatteryVoltage) ; 
fp, "BatteryVoltage 








fprintf (AbortLog 
fflush (AbortLogfp) ; 
ABORT_FLAG TRUE; 








if (BowLeakVoltage > MaxLeakVoltage) 


{ 


if (ABORT_FLAG 
{ 





FALSE) 


Sf\n", 


sf\n",BatteryVoltage) ; 


WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 
fprintf (AbortLogfp, "Abort Due to Leak in Bow 





fflush (AbortLogfp) ; 
ABORT_FLAG TRUE; 
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sf\n",BowLeakVoltage) ; 


if (MidLeakVoltage > MaxLeakVoltage) 
{ 





if (ABORT_FLAG == FALSE) 
{ 





WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 

fprintf (AbortLogfp, "Abort Due to Leak in Mid 
Compartment \n") ; 

fprintf (AbortLogfp, "MidLeakVoltage = %f\n",MidLeakVoltage) ; 

fflush (AbortLogfp) ; 

ABORT_FLAG = TRUE; 








if (SternLeakVoltage > MaxLeakVoltage) 
{ 

if (ABORT_FLAG == FALSE) 

{ 





WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 

fprintf (AbortLogfp, "Abort Due to Leak in Stern 
Compartment \n") ; 

fprintf (AbortLogfp, "SternLeakVoltage = 
sf\n",SternLeakVoltage) ; 

fflush (AbortLogfp) ; 

ABORT_FLAG = TRUE; 











/* After setting Time using date Execute 
rtc -s -l hw 


*/ 


time_of_day = time (NULL); 


// SPK 010306: Neutrino uses gmtime_r() instead of _gmtime() 
// old: _gmtime (&time_of_day, &tmbuf) ; 


gmtime_r(&time_of_day, &tmbuf) ; 
ftime (&timebuf) ; 

Month = tmbuf.tm_mon+l; 

Day = tmbuf.tm_mday; 

Year = tmbuf.tm_year+1900; 
Hour = tmbuf.tm_hour; 

Minute = tmbuf.tm_min; 





Second ((double) (tmbuf.tm_sect+ttimebuf.millitm/1000.0)); 





WriteBobShm (Bob_Shmid, Month, Day, Year, Hour,Minute, Second, 








NavFil_X,NavFil_Y,DepthEst, RDI_AltEst,GPS_Signal, 
LatDeg0, LongDeg0, HMR_Heading) ; 
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ol? 


oP ol? 


Fh Fh Fh 


ol? 


oe ol? 


Fh Qu Fh 





/* Write 





Estimates and Direct Measurements Her 


WriteNavShm (Nav_Shmid, 


Mont 











TtTc, 














fprintf (Outfp," 
Sd sd %d %d %f 
Sf Sf Sf SE Sd 
SE S£ %£\n", 
Nav_Iid, 
Hour, 
Minute, 
Second, 
P_Id, 
GAGg; 
P_qd, 
Pe; 
DI_Id, 
DI_Ug, 
DI_Vg, 
DI_Wg, 
DL UE; 
DI_Vf, 
DI_Wf, 








DHAAWDA ADE SEAS 





Nav_Id, 
h,Day, Year, Hour,Minute, Second, 
NavFil 











LLor, 
Li Bia Si ipsa, 
il_Bias_r, 





LatDeg, 


oP ol? 
Fh Fh Fh 


oe 


AP ol? 


ae 


ay 


Hh Fh QO. 
AP oP oO 
Fr FH Fh 
AJP oP ol? 
Fy FH Fh 
AJP oP oO 
Fr FH Fh 
AP oP oo 
Hh Fh QO. 
AP oP ol? 
Fr FH Fh 
AP oP ol 
Fy Fh Fh 
AP oP ol 
Fr FH Fh 
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AP ol? 


oe 


Fh Fh Fh 


oP ol 


oe 


Fh Fh Fh 


oP ol 


ole 


Fh Fh Fh 


oP ol 


oe 


Fh Fh Fh 


oP ol 


ole 


Fh Fh Fh 


RDI_AltRawComp, 
RDI_Al1tEst, 
RDI_AltDot, 
RDI_Heading, 
GPS_Id, 
GPS_Signal, 
Diff, 

NSv, 

ToC, 
LatDeg, 
LongDeg, 
LETS; 
DepthRaw, 
DepthEst, 
DepthDot, 
MP_phi, 
MP_theta, 
v_is, 

v_rs, 
NavFil_X, 
NavFil_yY, 
NavFil_psi, 
NavFil_r, 
NavFil_Ug, 
NavFil_Vg, 


























NavFil_Bias_r, 
Analog_Id, 





B 
BatteryVoltage, 
GPS_X, 

GPS_Y, 

HMR_Id, 
HMR_HeadingRaw, 
HMR_Heading, 
HMR_Pitch, 
HMR_Roll, 

KG_r, 








// SPK 09/01/05 
//KG_psi, 
psi, 


Pdop, 
Hdop, 
Vdop, 
Tdop, 


// SPK 10/24/05 
MP_XAccel, 
MP_YAccel, 





// SPK 11/22/05 
P_diag_out[1], 
P_diag_out[2], 


NavFil_Bias_psi, 


atteryVoltageRaw, 
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P_diag_ou 
P_diag_ou 
P_diag_ou 
P_diag_ou 
P_diag_ou 
P_diag_ou 


~ ON 


~ 


~ 


ot ct ct ct ct 
ONAN of Ww 
~ 


x 








// SPK 04/1 
P_1 8 out, 
P28. Out): 


7/06 





ReadNavFlagShm (NavFlag_Shmid, &Nav_Kill); 




















if (Nav_Kill) break; 
/* Don't Stop Navigator if ABORT */ 
/*if(ABORT_FLAG) break; */ 
if (Nav_Id>65534) 
Nav_Id = 0; 

} 

Nav_Id; 

t_count; 

fF(t_count == Hz) t_count = 0; 


(double) (t_count*dt); 











/* Clear Pending Proxies */ 

// SPK 010906: Neutrino equivalent is MsgReceive, preceeded 

// immediately by special timer code; but this requires 

// a channel ID vice a process ID; for now, just use 

// qnx_proxy_detach () 

// old: while (Creceive (LoopTimerProxy,0,0) == LoopTimerProxy) ; 
// SPK 011106: No longer using old code (or mig4nto version) 
//ceplace: qnx_proxy_detach (LoopTimerProxy) ; 

ConnectDetach( timerConId ); 

/* Get Rid of the Timer */ 


timer_delete (LoopTimerId) ; 


CloseNavDataFile() ; 

fclose (AbortLogfp) ; 

CloseNavShm (Nav_Shmid) ; 
CloseNavFlagShm (NavFlag_Shmid) ; 
CloseNetFlagShm (NetFlag_Shmid) ; 
CloseRDIShm(RDI_Shmid) ; 
CloseMotPakShm (MP_Shmid) ; 
CloseGPSShm(GPS_Shmid) ; 
CloseAnalogShm (Analog_Shmid) ; 
CloseBobShm (Bob_Shmid) ; 
CloseHMRShm (HMR_Shmid) ; 
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